import plotutils
from mathutils import fit_gaussian, Gaussian

cal_tool = TOOL_CALIBRATION

robot.set_tool(cal_tool)
robot.enable()
move_to_laser()


robot.set_motors_enabled(True)
robot.set_joint_motors_enabled(True)
initial_pos = robot.get_cartesian_pos()

#robot.align()  

run("calibration/ScanX")

pos1 = robot.get_cartesian_pos()
x1, l1 = closest_x, closest_y

print "Closest 1: ", [x1, l1]
print "Position 1: ", pos1


pj6 = robot_j6.position
if pj6>0:
    robot_j6.move(pj6 - 180.0)
else:    
    robot_j6.move(pj6 + 180.0)


run("calibration/ScanX")


x2, l2 = closest_x, closest_y
pos2 = robot.get_cartesian_pos()

print "Closest 2: ", [x2, l2]
print "Position 2: ", pos2


#Updates the tool
t=robot.get_tool_trsf(TOOL_DEFAULT)

xoff = (x1-x2)/2
yoff = (l2 - l1)/2 
xrot = math.degrees(math.atan(yoff/t[2]))
yrot = math.degrees(math.atan(xoff/t[2]))

print "Former tool: " + str(t)
t[0]=xoff 
t[1]=-yoff 
print "Offset: ", [t[0], t[1]]

print "CALIBRATED tool: " + str(t)
#robot.set_tool_trsf(t, TOOL_DEFAULT)

robot.set_tool(TOOL_DEFAULT)
d = robot.get_distance_to_pnt("pLaser")
if d<POSITION_TOLERANCE:
    print "Moving calibrated tool to laser"
    #move_to_laser()    
else:
    print "Cannot move calibrated tool to laser: too big offset"