###################################################################################################
#  Deployment specific global definitions - executed after startup.py
###################################################################################################


from ch.psi.pshell.serial import TcpDevice
from ch.psi.pshell.modbus import ModbusTCP


class RobotTCP(TcpDevice):    
    def __init__(self, name, server, timeout = 1000, retries = 1):
         TcpDevice.__init__(self, name, server) 
         self.timeout =  timeout       
         self.retries = retries
         self.header = None
         self.trailer = "\n"
         self.msg_id = 0

    def sendReceive(self, msg):
        tx = self.header if (self.header != None) else  self.header
        id = "%03d" % self.msg_id
        self.msg_id = (self.msg_id+1)%1000
        tx = tx + id + " " + msg
        self.getLogger().finer("TX = " +  str(tx))
        if (self.trailer != None): cmd = cmd + self.trailer
        rx = self.sendReceive(cmd, None, self.trailer , 0,  self.timeout, self.retries).strip()
        self.getLogger().finer("RX = " + str(rx))
        if rx[:3] != id:
            raise Exception("Received invalid message id")
        return rx[3:]
    
    def execute(self, command, *argv):        
        msg = str(command)
        for arg in argv:
            msg = msg + "," + str(arg)                 
        ret = self.sendReceive(msg)
        return ret;
        

    def mount(self, puck, sample):
        return self.execute('Mount', puck, sample)

add_device(RobotTCP("robot_tcp", "129.129.126.100:1000"), force = True)   




class RobotModbus(DeviceBase):
    def __init__(self, name):
         DeviceBase.__init__(self, name)
         robot_req.write(0)

    def execute(self, command, *argv):
        if robot_req.read() != 0:
            raise Exception("Ongoing command")
        if robot_ack.read() != 0:
            raise Exception("Robot is not ready")
        robot_cmd.write(command)
        args = [0] * robot_args.size
        index = 0        
        for arg in argv:
            args[index] = arg
            index = index + 1
            if index == robot_args.size:
                raise Exception("Invalid number of arguments")
        robot_args.write(to_array(args, 'i'))
        try:           
            self.request()         
            err = robot_ack.take()            
            if err == 1:
                ret = robot_ret.read()            
                return ret
            if err == 2:
                raise Exception("Invalid command: " + str(command))                
            raise Exception("Unknown error: " + str(err)) 
        finally:
            self.cancel_request()
    
    def request(self):
        robot_req.write(1)
        while robot_ack.read() == 0:
            time.sleep(0.001)

    def cancel_request(self):
        robot_req.write(0)
        while robot_ack.read() != 0:
            time.sleep(0.001)
        
    def mount(self, puck, sample):
        return self.execute('1', '1', puck, sample)        

add_device(RobotModbus("robot"), force = True)                
        




add_device(img.getContrast(), force = True)



def detect_pucks(ip):
    """
    """
    aux = grayscale(ip, in_place=False)
    threshold(aux,0,50)
    binary_fill_holes(aux)
    return analyse_particles(aux, 10000,50000,
            fill_holes = False, exclude_edges = True,print_table=True,
            output_image = "outlines", minCirc = 0.4, maxCirc = 1.0)

def detect_samples(ip):      
    """
    """  
    aux = grayscale(ip, in_place=False)
    invert(aux)
    subtract_background(aux)
    auto_threshold(aux)
    binary_open(aux)
    return analyse_particles(aux, 250,1000, 
            fill_holes = False, exclude_edges = True,print_table=True,
            output_image = "outlines", minCirc = 0.7, maxCirc = 1.0)


r,g,b = [0]*256,[0]*256,[0]*256
b[0]=0xFF
b[1]=0xFF ; g[1] = 0x80; r[1] = 0x80
outline_lut1 = (r,g,b)

r,g,b = [0]*256,[0]*256,[0]*256
g[0]=0x80;r[0]=0x80;
g[1]=0xFF ; r[1] = 0x80; b[1] = 0x80
outline_lut2 = (r,g,b)



from mathutils import estimate_peak_indexes, fit_gaussians, create_fit_point_list, Gaussian
import java.awt.Color as Color
 
import mathutils
mathutils.MAX_ITERATIONS = 100000

def fit(ydata, xdata = None, draw_plot = True):
    if xdata is None:
        xdata = frange(0, len(ydata), 1)
    max_y= max(ydata)    
    index_max = ydata.index(max_y)    
    max_x= xdata[index_max]
    print "Max index:" + str(index_max),
    print " x:" + str(max_x),
    print " y:" + str(max_y)
    
    if draw_plot:
        plots = plot([ydata],["data"],[xdata], title="Fit" )
        p = None if plots is None else plots[0]

    gaussians = fit_gaussians(ydata, xdata, [index_max,])
    if gaussians[0] is None:
        if draw_plot and (p is not None):
            p.addMarker(max_x, None, "Max="+str(round(max_x,4)), Color.GRAY)    
        print "Fitting error"
        return (None, None, None)
        
    (norm, mean, sigma) = gaussians[0]
    if draw_plot:
        fitted_gaussian_function = Gaussian(norm, mean, sigma)    
        scale_x = [float(min(xdata)), float(max(xdata)) ]
        points = max((len(xdata)+1), 100)
        resolution = (scale_x[1]-scale_x[0]) / points    
        fit_y = []
        fit_x = frange(scale_x[0],scale_x[1],resolution, True)
        for x in fit_x:
            fit_y.append(fitted_gaussian_function.value(x))            
        #Server
        if p is None:
            plot([ydata,fit_y],["data","fit"],[xdata,fit_x], title="Fit")             
            draw_plot = False
        else:
            p.addSeries(LinePlotSeries("fit"))
            p.getSeries(1).setData(fit_x, fit_y)            

    if abs(mean - xdata[index_max]) < abs((scale_x[0] + scale_x[1])/2):        
        if draw_plot:
            p.addMarker(mean, None, "Mean="+str(round(mean,4)), Color.MAGENTA.darker())
        print "Mean -> " +  str(mean)
        return (norm, mean, sigma)
    else:
        if draw_plot:
            p.addMarker(max_x, None, "Max="+str(round(max_x,4)), Color.GRAY)
        print "Invalid gaussian fit: " +  str(mean)
        return (None, None, None)