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

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


###################################################################################################
#  Scripted devices
###################################################################################################


run("RobotSC")
#run("RobotModbus")
#run("OneWire")

#Raspberry login: usr=pi  pwd=Buntschu

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


#TODO: The range should be set automatically reading LN2 sensor.
def set_led_range(room_temp = True):
    led_ctrl.config.maxValue = 0.40 if room_temp else 1.20
    led_ctrl.config.save()



###################################################################################################
#  Image processing utilities
###################################################################################################


from ijutils import *
from ch.psi.pshell.imaging.Overlays import *
import ch.psi.pshell.imaging.Pen as Pen

def in_roi(x,y):
    return math.hypot(x-roi_radius, y-roi_radius) < roi_radius


def integrate(ips):    
    roi =  get_roi()
    aux = None
    for i in range(len(ips)):
        if i==0:
            aux = new_image(roi[2], roi[3], image_type="float", title = "sum", fill_color = None)
        op_image(aux, ips[i], "add", float_result=True, in_place=True)    
    return aux

def average (ips):   
    aux = integrate(ips)     
    op_const(aux, "divide", len(ips), in_place=True)    
    return aux

def grab_frames(samples):
    frames = []            
    for i in range(samples):
        aux = get_image()        
        frames.append(aux)        
    return frames
    
def average_frames(samples = 1):
    return average(grab_frames(samples))

def integrate_frames(samples = 1):
    return integrate(grab_frames(samples))


roi_center = (800, 600)
roi_radius = 600

def get_roi():
    return (roi_center[0] - roi_radius, roi_center[1] - roi_radius, 2* roi_radius, 2*roi_radius)


def get_image():
    roi =  get_roi()
    ip = load_image(img.image)                            
    ret = sub_image(ip, roi[0], roi[1], roi[2], roi[3])
    grayscale(ret, do_scaling=False)
    return ret


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)

###################################################################################################
#  Math utilities
###################################################################################################


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)

context = get_context()