import plotutils
from mathutils import fit_gaussian, Gaussian


#robot.assert_tool(TOOL_CALIBRATION)
#cal_tool = TOOL_DEFAULT 
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.enable()
move_to_laser()

#robot.align()  


run("calibration/ScanY")

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

print "Closest 1: ", [x1, y1]
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/ScanY")


x2, y2 = closest_x, closest_y

print "Closest 2: ", [x2, y2]



off_x = x1 - x2

#robot.set_motors_enabled(True)
#robot_x.moveRel(off_x, -1)

#For composing cannot use tcp_p, need another auxiliary point. tcp_t is also destroyed.
robot.set_pnt(robot.get_cartesian_pos(), "pTemp")
robot.set_trsf([off_x, 0,0,0,0,0])
c=robot.compose("pTemp", FRAME_TABLE, "tcp_t" )
robot.set_pnt(c, "pTemp")
robot.movel("pTemp", cal_tool, DESC_SCAN, sync=True)


pos2 = robot.get_cartesian_pos()
print pos2

print "Position 2: ", pos2



#Updates the tool
xoff = (pos2[0]-pos1[0])/2
yoff = (pos2[1]-pos1[1])/2

#print "Offset: ", [xoff, yoff]

t=robot.get_tool_trsf(TOOL_DEFAULT)
t[0]=xoff 
t[1]=-yoff 
print "Offset: ", [t[0], t[1]]
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"
else:
    print "Cannot move calibrated tool to laser: toog big offset"