import linuxcnc
import gtk
from math import pi, cos, sin, tan, sqrt

probe_dia = 6
jump_height = 20
fmt = "{:.4f}"  # display format, 4 decimal places


s = linuxcnc.stat()
c = linuxcnc.command()

def ok_for_mdi():
    s.poll()
    return not s.estop and s.enabled and s.homed and (s.interp_state == linuxcnc.INTERP_IDLE)

def numeric(val):
	try:
		v = float(val)
	except:
		return None
	return v
	
def update(self):
	X1 = numeric(self.builder.get_object("X1").get_text())
	X2 = numeric(self.builder.get_object("X2").get_text())
	X3 = numeric(self.builder.get_object("X3").get_text())
	Y1 = numeric(self.builder.get_object("Y1").get_text())
	Y2 = numeric(self.builder.get_object("Y2").get_text())
	Y3 = numeric(self.builder.get_object("Y3").get_text())

	if None in (X1, X2, X3, Y1, Y2, Y3):
		return None
	
	temp = X2**2 + Y2**2
	bc = (X1**2 + Y1**2 - temp) / 2
	cd = (temp - X3**2 - Y3**2) / 2
	det = (X1 - X2) * (Y2 - Y3) - (X2 - X3) * (Y1 - Y2)

	if abs(det) < 1.0e-10:
		return None

	# Center of circle
	cx = (bc*(Y2 - Y3) - cd*(Y1 - Y2)) / det
	cy = ((X1 - X2) * cd - (X2 - X3) * bc) / det
	radius = ((cx - X1)**2 + (cy - Y1)**2)**.5
	
	self.builder.get_object('CentreX').set_text(fmt.format(cx))
	self.builder.get_object('CentreY').set_text(fmt.format(cy))
	self.builder.get_object('Diameter').set_text(fmt.format(radius * 2 - probe_dia))

class HandlerClass :

	def GoToZero(self, label, data=None):
		if ok_for_mdi():
			c.mode(linuxcnc.MODE_MDI)
			c.wait_complete() # wait until mode switch executed
			c.mdi("G0 Z[#<_Z> + %s]" % (jump_height))
			c.mdi("G0 X0 Y0")
		else:
			print("Can't enter MDI mode")


	def SetZero(self, label, data=None):
		cx = float(self.builder.get_object("CentreX").get_text())
		cy = float(self.builder.get_object("CentreY").get_text())
		s.poll()
		xoff = s.g5x_offset[0] + cx
		yoff = s.g5x_offset[1] + cy
		if ok_for_mdi():
			c.mode(linuxcnc.MODE_MDI)
			c.wait_complete() # wait until mode switch executed
			c.mdi("G10 L2 P0 X%s Y%s" % (xoff, yoff))
		else:
			print("Can't enter MDI mode")
			
	def Set1(self, label, data=None):
		s.poll()
		self.builder.get_object('X1').set_text(fmt.format(s.actual_position[0]))
		self.builder.get_object('Y1').set_text(fmt.format(s.actual_position[1]))
		update(self)

	def Set2(self, label, data=None):
		s.poll()
		self.builder.get_object('X2').set_text(fmt.format(s.actual_position[0]))
		self.builder.get_object('Y2').set_text(fmt.format(s.actual_position[1]))
		update(self)

	def Set3(self, label, data=None):
		s.poll()
		self.builder.get_object('X3').set_text(fmt.format(s.actual_position[0]))
		self.builder.get_object('Y3').set_text(fmt.format(s.actual_position[1]))
		update(self)

	def __init__(self, halcomp, builder, useropts):
		self.builder = builder

def get_handlers(halcomp,builder,useropts):
	return [HandlerClass(halcomp,builder,useropts)]