import plotutils
from mathutils import fit_gaussian, Gaussian


robot.assert_tool(TOOL_CALIBRATION)
robot.set_motors_enabled(True)
robot.set_joint_motors_enabled(True)


initial_pos = robot.get_cartesian_pos()

robot.enable()
move_to_laser()

robot.align()  


run("calibration/ScanYZ")

robot.set_motors_enabled(True)


first_y = robot_y.take()
first_z = robot_z.take()
first_y = ue.take()
first_j6 = robot_j6.take()
if first_y is None:
    raise Exception("Invalid YZ scan values in first scan")


robot.set_joint_motors_enabled(True)
if first_j6>0:
    robot_j6.moveRel(-180.0, -1)
else:
    robot_j6.moveRel(180.0, -1)    

    
robot.set_motors_enabled(True)
run("calibration/ScanYZ")

robot.set_motors_enabled(True)

second_y = robot_y.take()
second_z = robot_z.take()
second_y = ue.take()
second_j6 = robot_j6.take()
if second_y is None:
    raise Exception("Invalid XZ scan values in first scan")


#Updates the tool
xoff = (first_x - second_x)/2
yoff = (first_y - second_y)/2
t=robot.get_tool_trsf(TOOL_DEFAULT)
t[0]=xoff 
t[1]=-yoff 
robot.set_tool_trsf(t, TOOL_DEFAULT)