import plotutils
from mathutils import fit_gaussian, Gaussian


d = robot.get_distance_to_pnt("pLaser")
if d<0:
    raise Exception ("Error calculating distance to laser: " + str(d))

if d>20:
    raise Exception ("Should be near the laser position to perform the scan")

RANGE = [-2.0, 2.0] #[-1.5, 1.5]
STEP = 0.02
Z_OFFSET = 0 #-1.0
LATENCY = 0.025
BORDER_SIZE = 0.15

robot.enable()
robot.set_motors_enabled(True)
current_positon = robot_x.getPosition()
robot_z.moveRel(Z_OFFSET)

robot.setPolling(25)
try:
    ret = lscan(robot_x, ue.readable, RANGE[0], RANGE[1], STEP, latency = LATENCY, relative = True)
finally:    
    robot.setPolling(DEFAULT_ROBOT_POLLING)

d = ret.getReadable(0)

first_index = -1
last_index = -1
for i in range(len(d)):
    if not math.isnan(d[i]):
        if first_index<0:
            first_index = i  
        last_index = i  

if first_index == -1 or last_index < first_index:
    raise Exception("Invalid range")


remove = int(max(BORDER_SIZE, STEP) / STEP)

_range = [first_index+remove, last_index-remove]
if _range[1] <= _range[0]:
    raise Exception("Invalid range: " + str(_range))


center_index = int((_range[0] + _range[1])/2)
center_positon = ret.getPositions(0)[center_index]




y =  ret.getReadable(0)[_range[0] : _range[1]]
x =  ret.getPositions(0)[_range[0]: _range[1]]
#x = enforce_monotonic(x)
#(normalization, mean_val, sigma) = fit_gaussian([-v for v in y], x)


closest_x = x[y.indexOf(min(y))]
closest_y = y[y.indexOf(min(y))]



if closest_x is None or closest_x <= ret.getPositions(0)[first_index] or closest_x >= ret.getPositions(0)[last_index]:
    raise Exception("Invalid Fit")



center_offset = center_positon-closest_y
#center_offset = current_positon-closest_y



p=get_plots()[0]
p.addMarker(closest_y, p.AxisId.X, str(closest_y), Color.GREEN)

robot.set_motors_enabled(True)
robot_x.move(closest_x)