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/ScanXZ")

robot.set_motors_enabled(True)


first_x = robot_x.take()
first_z = robot_z.take()
first_y = ue.take()
first_j6 = robot_j6.take()
if first_y is None:
    raise Exception("Invalid XZ 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/ScanXZ")

robot.set_motors_enabled(True)

second_x = robot_x.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
robot.get_tool_trsf(robot.tool)
t[0]=xoff     #TODO: Why signal?
t[1]=-yoff    #TODO: Why signal?
robot.set_tool_trsf(t, robot.tool)