#!/usr/bin/env python 
"""
gright (C) 2009 Nick Drobchenko, nick@cnc-club.ru
based on gcode.py (C) 2007 hugomatic... 
based on addnodes.py (C) 2005,2007 Aaron Spike, aaron@ekips.org
based on dots.py (C) 2005 Aaron Spike, aaron@ekips.org
based on interp.py (C) 2005 Aaron Spike, aaron@ekips.org

This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
"""

###
###		Gcode tools v 1.2
###

import inkex, simplestyle, simplepath
import cubicsuperpath, simpletransform, bezmisc

import os
import math
import bezmisc
import re
import copy
import sys
import time
import cmath
import numpy

_ = inkex._


def bezierparameterize(((bx0,by0),(bx1,by1),(bx2,by2),(bx3,by3))):
	#parametric bezier
	ax,ay,bx,by,cx,cy,x0,y0 = 0, 0, 0, 0, 0, 0, 0, 0 
	if ( (bx0,by0)==(bx1,by1) and (bx2,by2)==(bx3,by3) or
		(bx0,by0)==(bx1,by1)==(bx2,by2) or (bx1,by1)==(bx2,by2)==(bx3,by3) ):
		x0=bx0
		cx=bx3-bx0
		y0=by0
		cy=by3-by0
	elif (bx2,by2)==(bx3,by3) :
	    x0=bx0
	    cx = (bx1-bx0)*2
	    bx = bx0-2*bx1+bx2
	    y0=by0
	    cy = (by1-by0)*2
	    by = by0-2*by1+by2
	elif (bx0,by0)==(bx1,by1) :
	    x0=bx1
	    cx = (bx2-bx1)*2
	    bx = bx1-2*bx2+bx3
	    y0=by1
	    cy = (by2-by1)*2
	    by = by1-2*by2+by3
	else:
		x0=bx0
		y0=by0
		cx=3*(bx1-x0)
		bx=3*(bx2-bx1)-cx
		ax=bx3-x0-cx-bx
		cy=3*(by1-y0)
		by=3*(by2-by1)-cy
		ay=by3-y0-cy-by
	return ax,ay,bx,by,cx,cy,x0,y0
bezmisc.bezierparameterize = bezierparameterize

################################################################################
###
###		Styles and additional parameters
###
################################################################################

math.pi2 = math.pi*2
straight_tolerance = 0.0001
straight_distance_tolerance = 0.0001
engraving_tolerance = 0.0001
loft_lengths_tolerance = 0.0000001
options = {}
defaults = {
		'header': '%\n(Generated by gcode_tools from inkscape.) \nM3 \n',
		'footer': 'M5 \nG00 X0.0000 Y0.0000 \nM2 \n(end)\n%'
}


loft_style = {
		'main curve':	simplestyle.formatStyle({ 'stroke': '#88f', 'fill': 'none', 'stroke-width':'1', 'marker-end':'url(#Arrow2Mend)' }),
	}

biarc_style = {
		'biarc0':	simplestyle.formatStyle({ 'stroke': '#88f', 'fill': 'none', 'stroke-width':'1' }),
		'biarc1':	simplestyle.formatStyle({ 'stroke': '#8f8', 'fill': 'none', 'stroke-width':'1' }),
		'line':		simplestyle.formatStyle({ 'stroke': '#f88', 'fill': 'none', 'stroke-width':'1' }),
		'area':		simplestyle.formatStyle({ 'stroke': '#777', 'fill': 'none', 'stroke-width':'0.1' }),
	}

biarc_style_dark = {
		'biarc0':	simplestyle.formatStyle({ 'stroke': '#33a', 'fill': 'none', 'stroke-width':'1' }),
		'biarc1':	simplestyle.formatStyle({ 'stroke': '#3a3', 'fill': 'none', 'stroke-width':'1' }),
		'line':		simplestyle.formatStyle({ 'stroke': '#a33', 'fill': 'none', 'stroke-width':'1' }),
		'area':		simplestyle.formatStyle({ 'stroke': '#222', 'fill': 'none', 'stroke-width':'0.3' }),
	}

biarc_style_dark_area = {
		'biarc0':	simplestyle.formatStyle({ 'stroke': '#33a', 'fill': 'none', 'stroke-width':'0.1' }),
		'biarc1':	simplestyle.formatStyle({ 'stroke': '#3a3', 'fill': 'none', 'stroke-width':'0.1' }),
		'line':		simplestyle.formatStyle({ 'stroke': '#a33', 'fill': 'none', 'stroke-width':'0.1' }),
		'area':		simplestyle.formatStyle({ 'stroke': '#222', 'fill': 'none', 'stroke-width':'0.3' }),
	}

biarc_style_i = {
		'biarc0':	simplestyle.formatStyle({ 'stroke': '#880', 'fill': 'none', 'stroke-width':'1' }),
		'biarc1':	simplestyle.formatStyle({ 'stroke': '#808', 'fill': 'none', 'stroke-width':'1' }),
		'line':		simplestyle.formatStyle({ 'stroke': '#088', 'fill': 'none', 'stroke-width':'1' }),
		'area':		simplestyle.formatStyle({ 'stroke': '#999', 'fill': 'none', 'stroke-width':'0.3' }),
	}

biarc_style_dark_i = {
		'biarc0':	simplestyle.formatStyle({ 'stroke': '#dd5', 'fill': 'none', 'stroke-width':'1' }),
		'biarc1':	simplestyle.formatStyle({ 'stroke': '#d5d', 'fill': 'none', 'stroke-width':'1' }),
		'line':		simplestyle.formatStyle({ 'stroke': '#5dd', 'fill': 'none', 'stroke-width':'1' }),
		'area':		simplestyle.formatStyle({ 'stroke': '#aaa', 'fill': 'none', 'stroke-width':'0.3' }),
	}



################################################################################
###
###		Common functions
###
################################################################################


def isnan(x): return type(x) is float and x != x
def isinf(x): inf = 1e5000; return x == inf or x == -inf

###
###		Just simple output function for better debugging
###

def print_(s=''):
	f = open(options.log_filename,"a")
	f.write(str(s))
	f.write("\n")
	f.close()




###
###		Point (x,y) operations
###
class P:
	def __init__(self, x, y=None):
		if not y==None:
			self.x, self.y = float(x), float(y)
		else:
			self.x, self.y = float(x[0]), float(x[1])
	def __add__(self, other): return P(self.x + other.x, self.y + other.y)
	def __sub__(self, other): return P(self.x - other.x, self.y - other.y)
	def __neg__(self): return P(-self.x, -self.y)
	def __mul__(self, other):
		if isinstance(other, P):
			return self.x * other.x + self.y * other.y
		return P(self.x * other, self.y * other)
	__rmul__ = __mul__
	def __div__(self, other): return P(self.x / other, self.y / other)
	def mag(self): return math.hypot(self.x, self.y)
	def unit(self):
		h = self.mag()
		if h: return self / h
		else: return P(0,0)
	def dot(self, other): return self.x * other.x + self.y * other.y
	def rot(self, theta):
		c = math.cos(theta)
		s = math.sin(theta)
		return P(self.x * c - self.y * s,  self.x * s + self.y * c)
	def angle(self): return math.atan2(self.y, self.x)
	def __repr__(self): return '%f,%f' % (self.x, self.y)
	def pr(self): return "%.2f,%.2f" % (self.x, self.y)
	def to_list(self): return [self.x, self.y]	


###
###		Functions to operate with CubicSuperPath
###

def point_to_csp_simple_bound_dist(p, csp):
	minx,miny,maxx,maxy = None,None,None,None
	for subpath in csp:
		for sp in subpath:
			for p_ in sp:
				minx = min(minx,p_[0]) if minx!=None else p_[0]
				miny = min(miny,p_[1]) if miny!=None else p_[1]
				maxx = max(maxx,p_[0]) if maxx!=None else p_[0]
				maxy = max(maxy,p_[1]) if maxy!=None else p_[1]
	return math.sqrt(max(minx-p[0],p[0]-maxx,0)**2+max(miny-p[1],p[1]-maxy,0)**2)

def point_to_csp_bound_dist(p, sp1, sp2 , max_needed_distance):
	d = point_to_csp_simple_bound_dist(p, [[ sp1,sp2]] )
	if max_needed_distance<=d :
		return d 
	sp = sp1[1:]+sp2[:2]
	d = None
	int_count = 0
	for i in xrange(4):
		x1, y1, dx, dy = sp[i-1][0],sp[i-1][1], sp[i][0]-sp[i-1][0], sp[i][1]-sp[i-1][1]
		if (dx**2+dy**2)>0 :
			d1 = min( (p[0]-x1)**2 + (p[1]-y1)**2, (p[0]-sp[i][0])**2 + (p[1]-sp[i][1])**2 )
			if 0<=((p[1]-y1)*dy+(p[0]-x1)*dx)/(dx**2+dy**2)<=1:
				d1 = min( d1, ((p[0]-x1)*dy+(y1-p[1])*dx)**2/(dx**2+dy**2) )
		else :
			d1 = (p[0]-x1)**2 + (p[1]-y1)**2
		d = min(d,d1) if d!=None else d1
		# Get intersections with horisontal or vertical lines that goes throught p and x
		if dx!=0 and 0<=(p[0]-x1)/dx<1 and (p[0]-x1)/dx*dy+y1>=p[1] :
			if p[0]-x1/dx*dy+y1==p[1]:
				return 0
			int_count +=1 
		 
		elif x1==p[0] : 
			if y1<=p[1]<=sp[i][1] or sp[i][1]<=p[1]<=y1:
				return 0
			elif y1<=p[1]: 
				x2, y2, dx2, dy2 = sp[i-1][0],sp[i-1][1], sp[i][0]-sp[i-1][0], sp[i][1]-sp[i-1][1]
				if dx2!=0 and dy!=0: 
					t1 = (x1-x2)/dx2
					if not (0<=t1<=1 and 0<=(y2-y1+t1*dy2)/dy<=1):
						int_count +=1 
				elif x1!=x2:
					int_count +=1 
	if int_count%2 == 0 :
		return math.sqrt(d) 
	else:
		return -math.sqrt(d)
		

				

def csp_at_t(sp1,sp2,t):
	bez = (sp1[1][:],sp1[2][:],sp2[0][:],sp2[1][:])
	return 	bezmisc.bezierpointatt(bez,t)

def cspbezsplit(sp1, sp2, t = 0.5):
	s1,s2 = bezmisc.beziersplitatt((sp1[1],sp1[2],sp2[0],sp2[1]),t)
	return [ [sp1[0][:], sp1[1][:], list(s1[1])], [list(s1[2]), list(s1[3]), list(s2[1])], [list(s2[2]), sp2[1][:], sp2[2][:]] ]
	
def cspbezsplitatlength(sp1, sp2, l = 0.5, tolerance = 0.01):
	bez = (sp1[1][:],sp1[2][:],sp2[0][:],sp2[1][:])
	t = bezmisc.beziertatlength(bez, l, tolerance)
	return cspbezsplit(sp1, sp2, t)	
	
def cspseglength(sp1,sp2, tolerance = 0.001):
	bez = (sp1[1][:],sp1[2][:],sp2[0][:],sp2[1][:])
	return bezmisc.bezierlength(bez, tolerance)	

def csplength(csp):
	total = 0
	lengths = []
	for sp in csp:
		for i in xrange(1,len(sp)):
			l = cspseglength(sp[i-1],sp[i])
			lengths.append(l)
			total += l			
	return lengths, total

def csp_segments(csp):
	l, seg = 0, [0]
	for sp in csp:
		for i in xrange(1,len(sp)):
			l += cspseglength(sp[i-1],sp[i])
			seg += [ l ] 

	if l>0 :
		seg = [seg[i]/l for i in xrange(len(seg))]
	return seg,l

# rebuild_csp Adds to csp control points makin it's segments looks like segs

def rebuild_csp (csp, segs, s=None):
	if s==None : s, l = csp_segments(csp)
	
	if len(s)>len(segs) : return None
	segs = segs[:]
	segs.sort()
	for i in xrange(len(s)):
		d = None
		for j in xrange(len(segs)):
			d = min( [abs(s[i]-segs[j]),j], d) if d!=None else [abs(s[i]-segs[j]),j]
		del segs[d[1]]
	for i in xrange(len(segs)):
		for j in xrange(0,len(s)):
			if segs[i]<s[j] : break
		if s[j]-s[j-1] != 0 :
			t = (segs[i] - s[j-1])/(s[j]-s[j-1])
			sp1,sp2,sp3 = cspbezsplit(csp[j-1],csp[j], t)
			csp = csp[:j-1] + [sp1,sp2,sp3] + csp[j+1:]
			s = s[:j] + [ s[j-1]*(1-t)+s[j]*t   ] + s[j:]
	return csp, s



###
###		Distance calculattion from point to arc
###

def csp_slope(sp1,sp2,t):
	bez = (sp1[1][:],sp1[2][:],sp2[0][:],sp2[1][:])
	return bezmisc.bezierslopeatt(bez,t)

def between(c,x,y):
		return x-straight_tolerance<=c<=y+straight_tolerance or y-straight_tolerance<=c<=x+straight_tolerance

def distance_from_point_to_arc(p, arc):
	P0,P2,c,a = arc
	dist = None
	p = P(p)
	r = (P0-c).mag()
	if r>0 :
		i = c + (p-c).unit()*r
		alpha = ((i-c).angle() - (P0-c).angle())
		if a*alpha<0: 
			if alpha>0:	alpha = alpha-math.pi2
			else: alpha = math.pi2+alpha
		if between(alpha,0,a) or min(abs(alpha),abs(alpha-a))<straight_tolerance : 
			return (p-i).mag(), [i.x, i.y]
		else : 
			d1, d2 = (p-P0).mag(), (p-P2).mag()
			if d1<d2 : 
				return (d1, [P0.x,P0.y])
			else :
				return (d2, [P2.x,P2.y])

def get_distance_from_csp_to_arc(sp1,sp2, arc1, arc2, tolerance = 0.01 ): # arc = [start,end,center,alpha]
	n, i = 10, 0
	d, d1, dl = (0,(0,0)), (0,(0,0)), 0
	while i<1 or (abs(d1[0]-dl[0])>tolerance and i<4):
		i += 1
		dl = d1*1	
		for j in range(n+1):
			t = float(j)/n
			p = csp_at_t(sp1,sp2,t) 
			d = min(distance_from_point_to_arc(p,arc1), distance_from_point_to_arc(p,arc2))
			d1 = max(d1,d)
		n=n*2
	return d1[0]

def get_distance_from_point_to_csp(p,sp1,sp2, tolerance = 0.01 ): 
	n, i = 10, 0
	d, dl = [None,(0,0)],[0,(0,0)]
	while i<2 or (abs(d[0]-dl[0])>tolerance and i<4):
		i += 1
		dl = d[:]	
		for j in range(n+1):
			t = float(j)/n
			cp = csp_at_t(sp1,sp2,t) 
			d = min( [(P(cp)-P(p)).mag(),t], d) if d[0]!=None else [(P(cp)-P(p)).mag(),t]
		n=n*2
	return d

def reverce_csp (csp):
	for i in range(len(csp)):
	 	n = []
	 	for j in csp[i]:
			n = [  [j[2][:],j[1][:],j[0][:]]  ] + n
	 	csp[i] = n[:]
	return csp

def cubic_solver(a,b,c,d):	
	if a!=0:
		#	Monics formula see http://en.wikipedia.org/wiki/Cubic_function#Monic_formula_of_roots
		a,b,c = (b/a, c/a, d/a)
		m = 2*a**3 - 9*a*b + 27*c
		k = a**2 - 3*b
		n = m**2 - 4*k**3
		w1 = -.5 + .5*cmath.sqrt(3)*1j
		w2 = -.5 - .5*cmath.sqrt(3)*1j
		m1 = pow(complex((m+cmath.sqrt(n))/2),1./3)
		n1 = pow(complex((m-cmath.sqrt(n))/2),1./3)
		x1 = -1./3 * (a + m1 + n1)
		x2 = -1./3 * (a + w1*m1 + w2*n1)
		x3 = -1./3 * (a + w2*m1 + w1*n1)
		return [x1,x2,x3]
	elif b!=0:
		det = c**2-4*b*d
		if det>0 :
			return [(-c+math.sqrt(det))/(2*b),(-c-math.sqrt(det))/(2*b)]
		elif d == 0 :
			return [-c/(b*b)] 	
		else :
			return [(-c+cmath.sqrt(det))/(2*b),(-c-cmath.sqrt(det))/(2*b)]
	elif c!=0 :
		return [-d/c]
	else : return []

def csp_line_intersection(l1,l2,sp1,sp2):
	dd=l1[0]
	cc=l2[0]-l1[0]
	bb=l1[1]
	aa=l2[1]-l1[1]
	if aa==cc==0 : return []
	if aa:
		coef1=cc/aa
		coef2=1
	else:
		coef1=1
		coef2=aa/cc
	bez = (sp1[1][:],sp1[2][:],sp2[0][:],sp2[1][:])
	ax,ay,bx,by,cx,cy,x0,y0=bezmisc.bezierparameterize(bez)
	a=coef1*ay-coef2*ax
	b=coef1*by-coef2*bx
	c=coef1*cy-coef2*cx
	d=coef1*(y0-bb)-coef2*(x0-dd)
	roots = cubic_solver(a,b,c,d)
	retval = []
	for i in roots :
		if type(i) is complex and i.imag==0:
			i = i.real
		if type(i) is not complex and 0<=i<=1:
			retval.append(i)
	return retval



################################################################################
###
###		Biarc function
###
###		Calculates biarc approximation of cubic super path segment
###		splits segment if needed or approximates it with straight line
###
################################################################################


def biarc(sp1, sp2, z1, z2, depth=0):
	def biarc_split(sp1,sp2, z1, z2, depth): 
		if depth<options.biarc_max_split_depth:
			sp1,sp2,sp3 = cspbezsplit(sp1,sp2)
			l1, l2 = cspseglength(sp1,sp2), cspseglength(sp2,sp3)
			if l1+l2 == 0 : zm = z1
			else : zm = z1+(z2-z1)*l1/(l1+l2)
			return biarc(sp1,sp2,z1,zm,depth+1)+biarc(sp2,sp3,z1,zm,depth+1)
		else: return [ [sp1[1],'line', 0, 0, sp2[1], [z1,z2]] ]

	P0, P4 = P(sp1[1]), P(sp2[1])
	TS, TE, v = (P(sp1[2])-P0), -(P(sp2[0])-P4), P0 - P4
	tsa, tea, va = TS.angle(), TE.angle(), v.angle()
	if TE.mag()<straight_distance_tolerance and TS.mag()<straight_distance_tolerance:	
		# Both tangents are zerro - line straight
		return [ [sp1[1],'line', 0, 0, sp2[1], [z1,z2]] ]
	if TE.mag() < straight_distance_tolerance:
		TE = -(TS+v).unit()
		r = TS.mag()/v.mag()*2
	elif TS.mag() < straight_distance_tolerance:
		TS = -(TE+v).unit()
		r = 1/( TE.mag()/v.mag()*2 )
	else:	
		r=TS.mag()/TE.mag()
	TS, TE = TS.unit(), TE.unit()
	tang_are_parallel = ((tsa-tea)%math.pi<straight_tolerance or math.pi-(tsa-tea)%math.pi<straight_tolerance )
	if ( tang_are_parallel  and 
				((v.mag()<straight_distance_tolerance or TE.mag()<straight_distance_tolerance or TS.mag()<straight_distance_tolerance) or
					1-abs(TS*v/(TS.mag()*v.mag()))<straight_tolerance)	):
				# Both tangents are parallel and start and end are the same - line straight
				# or one of tangents still smaller then tollerance

				# Both tangents and v are parallel - line straight
		return [ [sp1[1],'line', 0, 0, sp2[1], [z1,z2]] ]

	c,b,a = v*v, 2*v*(r*TS+TE), 2*r*(TS*TE-1)
	if v.mag()==0:
		return biarc_split(sp1, sp2, z1, z2, depth)
	asmall, bsmall, csmall = abs(a)<10**-10,abs(b)<10**-10,abs(c)<10**-10 
	if 		asmall and b!=0:	beta = -c/b
	elif 	csmall and a!=0:	beta = -b/a 
	elif not asmall:	 
		discr = b*b-4*a*c
		if discr < 0:	raise ValueError, (a,b,c,discr)
		disq = discr**.5
		beta1 = (-b - disq) / 2 / a
		beta2 = (-b + disq) / 2 / a
		if beta1*beta2 > 0 :	raise ValueError, (a,b,c,disq,beta1,beta2)
		beta = max(beta1, beta2)
	elif	asmall and bsmall:	
		return biarc_split(sp1, sp2, z1, z2, depth)
	alpha = beta * r
	ab = alpha + beta 
	P1 = P0 + alpha * TS
	P3 = P4 - beta * TE
	P2 = (beta / ab)  * P1 + (alpha / ab) * P3

	def calculate_arc_params(P0,P1,P2):
		D = (P0+P2)/2
		if (D-P1).mag()==0: return None, None
		R = D - ( (D-P0).mag()**2/(D-P1).mag() )*(P1-D).unit()
		p0a, p1a, p2a = (P0-R).angle()%(2*math.pi), (P1-R).angle()%(2*math.pi), (P2-R).angle()%(2*math.pi)
		alpha =  (p2a - p0a) % (2*math.pi)					
		if (p0a<p2a and  (p1a<p0a or p2a<p1a))	or	(p2a<p1a<p0a) : 
			alpha = -2*math.pi+alpha 
		if abs(R.x)>1000000 or abs(R.y)>1000000  or (R-P0).mag<options.min_arc_radius :
			return None, None
		else :	
			return  R, alpha
	R1,a1 = calculate_arc_params(P0,P1,P2)
	R2,a2 = calculate_arc_params(P2,P3,P4)
	if R1==None or R2==None or (R1-P0).mag()<straight_tolerance or (R2-P2).mag()<straight_tolerance	: return [ [sp1[1],'line', 0, 0, sp2[1], [z1,z2]] ]
	
	d = get_distance_from_csp_to_arc(sp1,sp2, [P0,P2,R1,a1],[P2,P4,R2,a2])
	if d > options.biarc_tolerance and depth<options.biarc_max_split_depth	 : return biarc_split(sp1, sp2, z1, z2, depth)
	else:
		if R2.mag()*a2 == 0 : zm = z2
		else : zm  = z1 + (z2-z1)*(R1.mag()*a1)/(R2.mag()*a2+R1.mag()*a1)  
		return [	[ sp1[1], 'arc', [R1.x,R1.y], a1, [P2.x,P2.y], [z1,zm] ], [ [P2.x,P2.y], 'arc', [R2.x,R2.y], a2, [P4.x,P4.y], [zm,z2] ]		]





				
################################################################################
###
###		Gcode tools class
###
################################################################################




class Gcode_tools(inkex.Effect):

	def __init__(self):
		inkex.Effect.__init__(self)
		self.OptionParser.add_option("-d", "--directory",					action="store", type="string", 		dest="directory", default="/home/",					help="Directory for gcode file")
		self.OptionParser.add_option("-f", "--filename",					action="store", type="string", 		dest="file", default="-1.0",						help="File name")			
		self.OptionParser.add_option("-u", "--Xscale",						action="store", type="float", 		dest="Xscale", default="1.0",						help="Scale factor X")	
		self.OptionParser.add_option("-v", "--Yscale",						action="store", type="float", 		dest="Yscale", default="1.0",						help="Scale factor Y")
		self.OptionParser.add_option("",   "--Zscale",						action="store", type="float", 		dest="Zscale", default="1.0",						help="Scale factor Z")				
		self.OptionParser.add_option("-x", "--Xoffset",						action="store", type="float", 		dest="Xoffset", default="0.0",						help="Offset along X")	
		self.OptionParser.add_option("-y", "--Yoffset",						action="store", type="float", 		dest="Yoffset", default="0.0",						help="Offset along Y")
		self.OptionParser.add_option("",   "--Zoffset",						action="store", type="float", 		dest="Zoffset", default="0.0",						help="Offset along Z")
		self.OptionParser.add_option("-s", "--Zsafe",						action="store", type="float", 		dest="Zsafe", default="0.5",						help="Z above all obstacles")
		self.OptionParser.add_option("-z", "--Zsurface",					action="store", type="float", 		dest="Zsurface", default="0.0",						help="Z of the surface")
		self.OptionParser.add_option("-c", "--Zdepth",						action="store", type="float", 		dest="Zdepth", default="-0.125",					help="Z depth of cut")
		self.OptionParser.add_option("",   "--Zstep",						action="store", type="float", 		dest="Zstep", default="-0.125",						help="Z step of cutting")		
		self.OptionParser.add_option("-p", "--feed",						action="store", type="float", 		dest="feed", default="4.0",							help="Feed rate in unit/min")

		self.OptionParser.add_option("",   "--biarc-tolerance",				action="store", type="float", 		dest="biarc_tolerance", default="1",				help="Tolerance used when calculating biarc interpolation.")				
		self.OptionParser.add_option("",   "--biarc-max-split-depth",		action="store", type="int", 		dest="biarc_max_split_depth", default="4",			help="Defines maximum depth of splitting while approximating using biarcs.")				

		self.OptionParser.add_option("",   "--tool-diameter",				action="store", type="float", 		dest="tool_diameter", default="3",					help="Tool diameter used for area cutting")		
		self.OptionParser.add_option("",   "--max-area-curves",				action="store", type="int", 		dest="max_area_curves", default="100",				help="Maximum area curves for each area")
		self.OptionParser.add_option("",   "--area-inkscape-radius",		action="store", type="float", 		dest="area_inkscape_radius", default="-10",			help="Radius for preparing curves using inkscape")
		self.OptionParser.add_option("",   "--unit",						action="store", type="string", 		dest="unit", default="G21 (All units in mm)",		help="Units")
		self.OptionParser.add_option("",   "--active-tab",					action="store", type="string", 		dest="active_tab", default="",						help="Defines which tab is active")

		self.OptionParser.add_option("",   "--generate_not_parametric_code",action="store", type="inkbool",		dest="generate_not_parametric_code", default=False,	help="Generated code will be not parametric.")		

		self.OptionParser.add_option("",   "--loft-distances",				action="store", type="string", 		dest="loft_distances", default="10",				help="Distances between paths.")
		self.OptionParser.add_option("",   "--loft-direction",				action="store", type="string", 		dest="loft_direction", default="crosswise",			help="Direction of loft's interpolation.")
		self.OptionParser.add_option("",   "--loft-interpolation-degree",	action="store", type="float",		dest="loft_interpolation_degree", default="2",		help="Which interpolation use to loft the paths smooth interpolation or staright.")

		self.OptionParser.add_option("",   "--min-arc-radius",				action="store", type="float", 		dest="min_arc_radius", default=".1",				help="All arc having radius less than minimum will be considered as straight line")		


		self.OptionParser.add_option("",   "--engraving-sharp-angle-tollerance",action="store", type="float",	dest="engraving_sharp_angle_tollerance", default="150",	help="All angles thar are less than engraving-sharp-angle-tollerance will be thought sharp")		
		self.OptionParser.add_option("",   "--engraving-max-dist",			action="store", type="float", 		dest="engraving_max_dist", default="10",			help="Distanse from original path where engraving is not needed (usualy it's cutting tool diameter)")		
		self.OptionParser.add_option("",   "--engraving-newton-iterations", action="store", type="int", 		dest="engraving_newton_iterations", default="4",	help="Number of sample points used to calculate distance")		
		self.OptionParser.add_option("",   "--engraving-draw-calculation-paths",action="store", type="inkbool",	dest="engraving_draw_calculation_paths", default=False,help="Draw additional graphics to debug engraving path")		
		self.OptionParser.add_option("",   "--engraving-cutter-shape-function",action="store", type="string", 	dest="engraving_cutter_shape_function", default="w",help="Cutter shape function z(w). Ex. cone: w. ")

		self.OptionParser.add_option("",   "--create-log",					action="store", type="inkbool", 	dest="log_create_log", default=False,	help="Create log files")
		self.OptionParser.add_option("",   "--log-filename",				action="store", type="string", 		dest="log_filename", default='',		help="Create log files")

		self.OptionParser.add_option("",   "--orientation-point1x",			action="store", type="float", 		dest="orientation_point1x", default='0',		help="Orientation point")
		self.OptionParser.add_option("",   "--orientation-point1y",			action="store", type="float", 		dest="orientation_point1y", default='0',		help="Orientation point")
		self.OptionParser.add_option("",   "--orientation-point2x",			action="store", type="float", 		dest="orientation_point2x", default='100',		help="Orientation point")
		self.OptionParser.add_option("",   "--orientation-point2y",			action="store", type="float", 		dest="orientation_point2y", default='0',		help="Orientation point")
		self.OptionParser.add_option("",   "--orientation-point3x",			action="store", type="float", 		dest="orientation_point3x", default='0',		help="Orientation point")
		self.OptionParser.add_option("",   "--orientation-point3y",			action="store", type="float", 		dest="orientation_point3y", default='100',		help="Orientation point")
		self.OptionParser.add_option("",   "--orientation-scale",			action="store", type="float", 		dest="orientation_scale", default='2.8222222',	help="Orientation points initial scale")


	def parse_curve(self, p, w = None, f = None):
			c = []
			if len(p)==0 : 
				inkex.errormsg(_("Nothing to do. Check that all paths are actually paths."))
				return []
			p = self.transform_csp(p)

			### Sort to reduce Rapid distance	
			k = range(1,len(p))
			keys = [0]
			while len(k)>0:
				end = p[keys[-1]][-1][1]
				dist = None
				for i in range(len(k)):
					start = p[k[i]][0][1]
					dist = max(   ( -( ( end[0]-start[0])**2+(end[1]-start[1])**2 ) ,i)    ,   dist )
				keys += [k[dist[1]]]
				del k[dist[1]]
			for k in keys:
				subpath = p[k]
				c += [ [    [subpath[0][1][0],subpath[0][1][1]]   , 'move', 0, 0] ]
				for i in range(1,len(subpath)):
					sp1 = [  [subpath[i-1][j][0], subpath[i-1][j][1]] for j in range(3)]
					sp2 = [  [subpath[i  ][j][0], subpath[i  ][j][1]] for j in range(3)]
					c += biarc(sp1,sp2,0,0) if w==None else biarc(sp1,sp2,-f(w[k][i-1]),-f(w[k][i]))
				c += [ [ [subpath[-1][1][0],subpath[-1][1][1]]  ,'end',0,0] ]
			return c



	def draw_curve(self, curve, group=None, style=biarc_style):
		if group==None:
			group = inkex.etree.SubElement( self.biarcGroup, inkex.addNS('g','svg') )
		s, arcn = '', 0
		
		
		a,b,c = [0.,0.], [1.,0.], [0.,1.]
		k = (b[0]-a[0])*(c[1]-a[1])-(c[0]-a[0])*(b[1]-a[1])
		a,b,c = self.transform(a,True), self.transform(b,True), self.transform(c,True)
		if ((b[0]-a[0])*(c[1]-a[1])-(c[0]-a[0])*(b[1]-a[1]))*k > 0 : reverse_angle = 1
		else : reverse_angle = -1 
		for sk in curve:
			si = sk[:]
			si[0], si[2] = self.transform(si[0],True), (self.transform(si[2],True) if type(si[2])==type([]) and len(si[2])==2 else si[2])
			
			if s!='':
				if s[1] == 'line':
					inkex.etree.SubElement(	group, inkex.addNS('path','svg'), 
							{
								'style': style['line'],
								'd':'M %s,%s L %s,%s' % (s[0][0], s[0][1], si[0][0], si[0][1]),
								'comment': str(s)
							}
						)
				elif s[1] == 'arc':
					arcn += 1
					sp = s[0]
					c = s[2]
					s[3] = s[3]*reverse_angle
						
					a =  ( (P(si[0])-P(c)).angle() - (P(s[0])-P(c)).angle() )%math.pi2 #s[3]
					if s[3]*a<0: 
							if a>0:	a = a-math.pi2
							else: a = math.pi2+a
					r = math.sqrt( (sp[0]-c[0])**2 + (sp[1]-c[1])**2 )
					a_st = ( math.atan2(sp[0]-c[0],- (sp[1]-c[1])) - math.pi/2 ) % (math.pi*2)
					if a>0:
						a_end = a_st+a
					else: 
						a_end = a_st*1
						a_st = a_st+a	
					inkex.etree.SubElement(	group, inkex.addNS('path','svg'), 
						 {
							'style': style['biarc%s' % (arcn%2)],
							 inkex.addNS('cx','sodipodi'):		str(c[0]),
							 inkex.addNS('cy','sodipodi'):		str(c[1]),
							 inkex.addNS('rx','sodipodi'):		str(r),
							 inkex.addNS('ry','sodipodi'):		str(r),
							 inkex.addNS('start','sodipodi'):	str(a_st),
							 inkex.addNS('end','sodipodi'):		str(a_end),
							 inkex.addNS('open','sodipodi'):	'true',
							 inkex.addNS('type','sodipodi'):	'arc',
							'comment': str(s)
						})
			s = si
	

	def check_dir(self):
		if (os.path.isdir(self.options.directory)):
			if (os.path.isfile(self.options.directory+'/header')):
				f = open(self.options.directory+'/header', 'r')
				self.header = f.read()
				f.close()
			else:
				self.header = defaults['header']
			if (os.path.isfile(self.options.directory+'/footer')):
				f = open(self.options.directory+'/footer','r')
				self.footer = f.read()
				f.close()
			else:
				self.footer = defaults['footer']
		
			self.header += self.options.unit + ( """
#4  = %f (Feed)
#5  = 1 (Scale xy)
#7  = %f (Scale z)
#8  = 0 (Offset x)
#9  = 0 (Offset y)
#10 = %f (Offset z)
#11 = %f (Safe distanse)\n""" % (self.options.feed, self.options.Zscale, self.options.Zoffset, self.options.Zsafe)
			if not self.options.generate_not_parametric_code else "" )
			return True
		else: 
			inkex.errormsg(_("Directory does not exist!"))
			return False
	
	def generate_gcode(self, curve, depth):
		def c(c):
			c = [c[i] if i<len(c) else None for i in range(6)]
			if c[5] == 0 : c[5]=None
			if self.options.generate_not_parametric_code:
				s,s1 = [" X", " Y", " Z", " I", " J", " K"], ["","","","","",""]
				m,a = [1,1,self.options.Zscale,1,1,self.options.Zscale], [0,0,self.options.Zoffset,0,0,0]
			else :
				s,s1 = [" X[", " Y[", " Z[", " I[", " J[", " K["], [ "*#5+#8]", "*#5+#9]", "*#7+#10]", "*#5]",  "*#5]", "*#7]"]
				m,a = [1,1,1,1,1,1], [0,0,0,0,0,0]
			r = ''	
			for i in range(6):
				if c[i]!=None:
					r += s[i] + ("%f" % (c[i]*m[i]+a[i])) + s1[i]
			return r
		if len(curve)==0 : return ""	
		g, lg, zs, f = '', 'G00', self.options.Zsafe, " F%f"%self.options.feed if self.options.generate_not_parametric_code else "F#4"
		for i in range(1,len(curve)):
			s, si = curve[i-1], curve[i]
			feed = f if lg not in ['G01','G02','G03'] else ''
			if s[1]	== 'move':
				g += "G00" + c([None,None,zs]) + "\nG00" + c(si[0]) + "\n"
				lg = 'G00'
			elif s[1] == 'end':
				g += "G00" + c([None,None,zs]) + "\n"
				lg = 'G00'
			elif s[1] == 'line':
				if lg=="G00": g += "G01" + c([None,None,s[5][0]+depth]) + feed +"\n"	
				g += "G01" +c(si[0]+[s[5][1]+depth]) + feed + "\n"
				lg = 'G01'
			elif s[1] == 'arc':
				r = [(s[2][0]-s[0][0]), (s[2][1]-s[0][1])]
				if (r[0]**2 + r[1]**2)>self.options.min_arc_radius:
					r1, r2 = (P(s[0])-P(s[2])), (P(si[0])-P(s[2]))
					if abs(r1.mag()-r2.mag()) < 0.001 :
						if lg=="G00": g += "G01" + c([None,None,s[5][0]+depth]) + feed + "\n"
						g += ("G02" if s[3]<0 else "G03") + c(si[0]+[ s[5][1]+depth, (s[2][0]-s[0][0]),(s[2][1]-s[0][1])  ]) + feed + "\n"
					else:
						r = (r1.mag()+r2.mag())/2
						g += ("G02" if s[3]<0 else "G03") + c(si[0]+[s[5][1]+depth]) + " R%f" % (r) + feed  + "\n"
					lg = 'G02'
				else:
					if lg=="G00": g += "G01" + c([None,None,s[5][0]+depth]) + "\n"	
					g += "G01" +c(si[0]+[s[5][1]+depth]) + feed + "\n"
					lg = 'G01'
		if si[1] == 'end':
			g += "G00" + c([None,None,zs]) + "\n"
		return g
	
	def apply_transforms(self,g,csp):
		root = self.document.getroot()
		i=0
		while (g.getparent()!=root) and i<10:
			if 'transform' in g.keys():
				trans = g.get('transform')
				trans = simpletransform.parseTransform(trans)
				simpletransform.applyTransformToPath(trans, csp)
			g=g.getparent()

		return csp


	def transform(self,source_point, reverse=False):
		def search_in_group(g):
			p = []
			for i in g:
				if i.tag == inkex.addNS("g",'svg') and i.get("comment") == "Gcode tools orientation point":
					p += [i]
				elif i.tag == inkex.addNS("g",'svg'):
					p1 = search_in_group(i)
					if len(p1)==3: return p1
				if len(p)==3 : return p
			return []
				 
		if self.transform_matrix==None:
			# Search orientation points in current Layer firs then in whole painting 
			for g in [self.current_layer, self.document.getroot()] if self.current_layer is not None else [self.document.getroot()]:
				p = search_in_group(g)
				if len(p)==3 : break
			if len(p)==3:
				points = []
				for g in p:
					point = [[],[]]	
					for node in g:
						if node.get('comment') == "Gcode tools orientation point arrow":
							point[0] = self.apply_transforms(node,cubicsuperpath.parsePath(node.get("d")))[0][0][1]
						if node.get('comment') == "Gcode tools orientation point text":
							r = re.match(r'(?i)\s*\(\s*(\d*(,|\.)*\d*)\s*;\s*(\d*(,|\.)*\d*)\)\s*',node.text)
							point[1] = [float(r.group(1)),float(r.group(3))]
					if point[0]!=[] and point[1]!=[]:	points += [point]
				if len(points)==3:
					matrix = numpy.array([
								[points[0][0][0], points[0][0][1], 1, 0, 0, 0, 0, 0, 0],
								[0, 0, 0, points[0][0][0], points[0][0][1], 1, 0, 0, 0],
								[0, 0, 0, 0, 0, 0, points[0][0][0], points[0][0][1], 1],
								[points[1][0][0], points[1][0][1], 1, 0, 0, 0, 0, 0, 0],
								[0, 0, 0, points[1][0][0], points[1][0][1], 1, 0, 0, 0],
								[0, 0, 0, 0, 0, 0, points[1][0][0], points[1][0][1], 1],
								[points[2][0][0], points[2][0][1], 1, 0, 0, 0, 0, 0, 0],
								[0, 0, 0, points[2][0][0], points[2][0][1], 1, 0, 0, 0],
								[0, 0, 0, 0, 0, 0, points[2][0][0], points[2][0][1], 1]
							])
					if numpy.linalg.det(matrix)!=0 :
						m = numpy.linalg.solve(matrix,
							numpy.array(
								[[points[0][1][0]], [points[0][1][1]], [1], [points[1][1][0]], [points[1][1][1]], [1], [points[2][1][0]], [points[2][1][1]], [1]]	
										)
							).tolist()
						self.transform_matrix = [[m[j*3+i][0] for i in range(3)] for j in range(3)]
					else : self.transform_matrix = [[1,0,0],[0,1,0],[0,0,1]]
				else : self.transform_matrix = [[1,0,0],[0,1,0],[0,0,1]]
			else : self.transform_matrix = [[1,0,0],[0,1,0],[0,0,1]]
			self.transform_matrix_reverse = numpy.linalg.inv(self.transform_matrix).tolist()		
			print_("\n Transformation matrixes:")
			print_(self.transform_matrix)
			print_(self.transform_matrix_reverse)
			
		x,y = source_point[0],	source_point[1]
		if not reverse :
			t = self.transform_matrix
			return [t[0][0]*x+t[0][1]*y+t[0][2], t[1][0]*x+t[1][1]*y+t[1][2]]
		else :
			t = self.transform_matrix_reverse
			return [t[0][0]*x+t[0][1]*y+t[0][2], t[1][0]*x+t[1][1]*y+t[1][2]]
			
			


	def transform_csp(self,csp):
		for i in xrange(len(csp)):
			for j in xrange(len(csp[i])): 
				for k in xrange(len(csp[i][j])): 
					csp[i][j][k] = self.transform(csp[i][j][k])
		return csp
################################################################################
###
###		Effect
###
###		Main function of Gcode tools class
###
################################################################################

	
	def effect(self):
		
		global options
		options = self.options
			
		# define print_ function 
		global print_
		if self.options.log_create_log :
			try :
				if os.path.isfile(self.options.log_filename) : os.remove(self.options.log_filename)
				f = open(self.options.log_filename,"a")
				f.write("Gcode tools log file.\nStarted at %s.\n%s\n" % (time.strftime("%d.%m.%Y %H:%M:%S"),options.log_filename))
				f.write("%s tab is active.\n" % self.options.active_tab)
				f.close()
			except :
				print_  = lambda x : None 
		else : print_  = lambda x : None 
		
		self.transform_matrix = None	
			
################################################################################
###
###		Curve to Gcode
###
################################################################################
		if self.options.active_tab not in ['"path-to-gcode"', '"area"', '"engraving"', '"orientation"']:
			inkex.errormsg(_("Select one of the active tabs - Path to Gcode, Area, Engraving or Orientation."))
			return
		elif self.options.active_tab == '"path-to-gcode"':

			if len(self.options.ids)<=0:
				inkex.errormsg(_("This extension requires at least one selected path."))
				return

			if not self.check_dir() : 
				return
			gcode = self.header

			#	Set group
			self.biarcGroup = inkex.etree.SubElement( self.selected[self.options.ids[0]].getparent(), inkex.addNS('g','svg') )
			options.Group = self.biarcGroup
			p = []	
			
			for id, node in self.selected.iteritems():
				if node.tag == inkex.addNS('path','svg'):
					csp = cubicsuperpath.parsePath(node.get("d"))
					if 'transform' in node.keys():
						trans = node.get('transform')
						trans = simpletransform.parseTransform(trans)
						simpletransform.applyTransformToPath(trans, csp)
					p += csp
#					gcode += '(Found path %s)\n' % node.get('id').replace('(','').replace(')','').replace('\n','')
			curve = self.parse_curve(p)
			self.draw_curve(curve)
			if self.options.Zstep == 0 : Zstep = 1
			for step in range( 0,  int(math.ceil( abs( (self.options.Zdepth-self.options.Zsurface)/self.options.Zstep )) ) ):
				Zpos = max(		self.options.Zdepth,		 self.options.Zsurface - abs(self.options.Zstep*(step+1))	)
				gcode += self.generate_gcode(curve,Zpos)
			gcode += self.footer
			try: 	
				f = open(self.options.directory+'/'+self.options.file, "w")	
				f.write(gcode)
				f.close()							
			except:
				inkex.errormsg(_("Can not write to specified file!"))
				return

################################################################################
###
###		Calculate area curves
###
################################################################################

		elif self.options.active_tab == '"area"' :
			if self.options.tool_diameter<=0 : 
							inkex.errormsg(_("Tool diameter must be > 0!"))				
							return
			for id, node in self.selected.iteritems():
				self.biarcGroup = inkex.etree.SubElement( node.getparent(), inkex.addNS('g','svg') )
				break
									
			for id, node in self.selected.iteritems():
				if node.tag == inkex.addNS('path','svg'):
					d = node.get('d')
					csp = cubicsuperpath.parsePath(d)
					# Finding top most point in path (min y value)
					my = (None, 0, 0, 0)
					for i in range(len(csp)):
						for j in range(1,len(csp[i])):
							ax,ay,bx,by,cx,cy,x0,y0 = bezmisc.bezierparameterize((csp[i][j-1][1],csp[i][j-1][2],csp[i][j][0],csp[i][j][1]))
							if ay == 0 :
								roots = [ -cy/(2*by) ] if by !=0 else []
							else:
								det = (2.0*by)**2 - 4.0*(3*ay*cy)
								roots = [ (-2*by+math.sqrt(det))/(6*ay), (-2*by+math.sqrt(det))/(6*ay) ] if det>=0 else []
							roots += [1,0]	
							for t in roots :
								if 0<=t<=1:
									y = ay*(t**3)+by*(t**2)+cy*t+y0  
									x = ax*(t**3)+bx*(t**2)+cx*t+x0  
									if my[0]>y or my[0] == None : 
										my = (y,i,j,t,x)
									elif my[0]==y and x>my[4] : 
										my = (y,i,j,t,x)
										
					if my[0]!=None :
 					 	subp = csp[my[1]]
 					 	del csp[my[1]]
 					 	j = my[2]
 					 	if my[3] in [0,1]:
 					 		if my[3] == 0: j=j-1
	 					 	subp[-1][2], subp[0][0] = subp[-1][1], subp[0][1]
 					 		subp = [ [subp[j][1], subp[j][1], subp[j][2]] ] + subp[j+1:] + subp[:j] + [ [subp[j][0], subp[j][1], subp[j][1]] ]
						else: 					 		
	 						sp1,sp2,sp3 = cspbezsplit(subp[j-1],subp[j],my[3])
	 						subp[-1][2], subp[0][0] = subp[-1][1], subp[0][1]
							subp = [ [ sp2[1], sp2[1],sp2[2] ] ] + [sp3] + subp[j+1:] + subp[:j-1] + [sp1] + [[ sp2[0], sp2[1],sp2[1] ]] 					 	  
 					 	csp = [subp] + csp
						# Reverce path if needed
						st,end = [],[]
						for i in range(1,len(subp)):
							pst = P( csp_at_t(subp[i-1],subp[i],.1) )
							pend = P( csp_at_t(subp[-i-1],subp[-i],.9) )
							if st==[] and (pst-P(subp[0][1])).mag()>straight_tolerance:
								st = pst - P(subp[0][1])
							if end==[] and (pend-P(subp[0][1])).mag()>straight_tolerance:
								end = pend - P(subp[0][1])
							if st!=[] and end!=[]: break
						if math.atan2(st.x,st.y) % math.pi2 < math.atan2(end.x,end.y) % math.pi2 :
							for i in range(len(csp)):
							 	n = []
							 	for j in csp[i]:
							 		n = [  [j[2][:],j[1][:],j[0][:]]  ] + n
							 	csp[i] = n[:]

								
 					 	
 					 	
					d = cubicsuperpath.formatPath(csp)
					d = re.sub(r'(?i)(m[^mz]+)',r'\1 Z ',d)
					d = re.sub(r'(?i)\s*z\s*z\s*',r' Z ',d)
					d = re.sub(r'(?i)\s*([A-Za-z])\s*',r' \1 ',d)
					r = self.options.area_inkscape_radius 							
					sign=1 if r>0 else -1
					a = self.transform([0,0],True)
					b = self.transform([self.options.tool_diameter,0],True)
					tool_d = math.sqrt( (b[0]-a[0])**2 + (b[1]-a[1])**2 )
					c = self.transform([r,0],True)
					r = math.sqrt( (c[0]-a[0])**2 + (c[1]-a[1])**2 )
					print_("tool_diameter=%10.3f, r=%10.3f" % (tool_d, r))
					for i in range(self.options.max_area_curves):
						radius = - tool_d * (i+0.5) * sign
						if abs(radius)>abs(r): 
							radius = -r
							
						inkex.etree.SubElement(	self.biarcGroup, inkex.addNS('path','svg'), 
										{
											 inkex.addNS('type','sodipodi'):	'inkscape:offset',
											 inkex.addNS('radius','inkscape'):	str(radius),
											 inkex.addNS('original','inkscape'):	d,
											'style':				biarc_style_i['area']
										})
						if radius == -r : break 
	

################################################################################
###
###		Engraving
###
################################################################################

		elif self.options.active_tab == '"engraving"' :
		
################################################################################			
###			
###		To find center of cutter a system of non linear equations will be solved
###		using Newton's method 
###			
################################################################################
			
			if not self.check_dir() : return
			gcode = self.header

			def inv(a): # invert matrix 3x3
				det = float(a[0][0]*a[1][1]*a[2][2] + a[0][1]*a[1][2]*a[2][0] + a[1][0]*a[2][1]*a[0][2] - a[0][2]*a[1][1]*a[2][0] - a[0][0]*a[2][1]*a[1][2] - a[0][1]*a[2][2]*a[1][0])
				if det==0: return None
				return	[  
					[  (a[1][1]*a[2][2] - a[2][1]*a[1][2])/det,  -(a[0][1]*a[2][2] - a[2][1]*a[0][2])/det,  (a[0][1]*a[1][2] - a[1][1]*a[0][2])/det ], 
					[ -(a[1][0]*a[2][2] - a[2][0]*a[1][2])/det,   (a[0][0]*a[2][2] - a[2][0]*a[0][2])/det, -(a[0][0]*a[1][2] - a[1][0]*a[0][2])/det ], 
					[  (a[1][0]*a[2][1] - a[2][0]*a[1][1])/det,  -(a[0][0]*a[2][1] - a[2][0]*a[0][1])/det,  (a[0][0]*a[1][1] - a[1][0]*a[0][1])/det ]
				]

			def find_cutter_center((x1,y1),(nx1,ny1), sp1,sp2,t3 = .5):
				bez = (sp1[1][:],sp1[2][:],sp2[0][:],sp2[1][:])
				ax,ay,bx,by,cx,cy,dx,dy=bezmisc.bezierparameterize(bez)
				fx=ax*(t3*t3*t3)+bx*(t3*t3)+cx*t3+dx
				fy=ay*(t3*t3*t3)+by*(t3*t3)+cy*t3+dy
			
				f1x = -(3*ay*(t3*t3)+2*by*t3+cy)
				f1y = 3*ax*(t3*t3)+2*bx*t3+cx
				
				if (ny*f1x-nx*f1y) != 0 :
					t1 = ((fy-y1)*f1x - (fx-x1)*f1y)/(ny*f1x-nx*f1y)
					t2 = (x1-fx-t1*nx)/f1x if f1x != 0 else (y1-fy-t1*ny)/f1y
				if (ny*f1x-nx*f1y)==0 or t1<0 or t2<0 : 	
					t1 = self.options.tool_diameter
					t2 = self.options.tool_diameter/math.sqrt(f1x*f1x+f1y*f1y)
					
				t = [ t1, t2, t3 ]					
				i = 0
				F = [0.,0.,0.]
				F1 = [[0.,0.,0.],[0.,0.,0.],[0.,0.,0.]]
				while i==0 or abs(F[0])+abs(F[1])+math.sqrt(abs(F[2])) >engraving_tolerance and i<10:
					t1,t2,t3 = t[0],t[1],t[2]
					fx=ax*(t3*t3*t3)+bx*(t3*t3)+cx*t3+dx
					fy=ay*(t3*t3*t3)+by*(t3*t3)+cy*t3+dy
					f1x=3*ax*(t3*t3)+2*bx*t3+cx
					f1y=3*ay*(t3*t3)+2*by*t3+cy
					i+=1
					
					tx = fx-x1-nx1*t1
					ty = fy-y1-ny1*t1
					
					F[0] = x1+nx1*t1-fx+t2*f1y
					F[1] = y1+ny1*t1-fy-t2*f1x
					F[2] = t1*t1 - tx*tx -ty*ty					
					
					F1[0][0] = nx1
					F1[0][1] = f1y
					F1[0][2] = -f1x+t2*(6*ay*t3+2*by)
					
					F1[1][0] = ny1
					F1[1][1] = -f1x
					F1[1][2] = -f1y-t2*(6*ax*t3+2*bx)
					
					F1[2][0] = 2*t1+2*nx1*tx +2*ny1*ty
					F1[2][1] = 0
					F1[2][2] = -2*f1x*tx -2*f1y*ty

					F1 = inv(F1)
				
					if (	 isnan(F[0]) or isnan(F[1]) or isnan(F[2]) or 
							 isinf(F[0]) or isinf(F[1]) or  isinf(F[2]) ):
						return t+[1e100,i]	
				
					if F1!= None :
						t[0] -=  F1[0][0]*F[0] + F1[0][1]*F[1] + F1[0][2]*F[2]
						t[1] -=  F1[1][0]*F[0] + F1[1][1]*F[1] + F1[1][2]*F[2]
						t[2] -=  F1[2][0]*F[0] + F1[2][1]*F[1] + F1[2][2]*F[2]
					else: break	
					
				return t+[abs(F[0])+abs(F[1])+math.sqrt(abs(F[2])),i]	

			def csp_simpe_bound(csp):
				minx,miny,maxx,maxy = None,None,None
				for subpath in csp:
					for sp in subpath : 
						for p in sp:
							minx = min(minx,p[0]) if minx!=None else p[0]
							miny = min(miny,p[1]) if miny!=None else p[1]
							maxx = max(maxx,p[0]) if maxx!=None else p[0]
							maxy = max(maxy,p[1]) if maxy!=None else p[1]
				return minx,miny,maxx,maxy		
						
						
			self.Group = inkex.etree.SubElement( self.selected[self.options.ids[0]].getparent(), inkex.addNS('g','svg') )
			cspe =[]
			we = []
			for id, node in self.selected.iteritems():
				if node.tag == inkex.addNS('path','svg'):
					cspi = cubicsuperpath.parsePath(node.get('d'))

					for j in xrange(len(cspi)):
						# Remove zerro length segments
						i = 1
						while i<len(cspi[j]):
							if abs(cspi[j][i-1][1][0]-cspi[j][i][1][0])<engraving_tolerance and abs(cspi[j][i-1][1][1]-cspi[j][i][1][1])<engraving_tolerance:
								cspi[j][i-1][2] = cspi[j][i][2]
								del cspi[j][i]
							else:
								i += 1
					for csp in cspi:
						#	Create list containing normlas and points
						nl = []
						for i in range(1,len(csp)):
							n, n1 = [], []
							sp1, sp2 = csp[i-1], csp[i]
							bez = (sp1[1][:],sp1[2][:],sp2[0][:],sp2[1][:])
							for ti in [.0,.25,.75,1.]:
								#	Is following string is nedded or not??? (It makes t depend on form of the curve) 
								#ti = bezmisc.beziertatlength(bez,ti)	
								x1,y1 = bezmisc.bezierpointatt(bez,ti)
								nx,ny = bezmisc.bezierslopeatt(bez,ti)
								nx,ny = -ny/math.sqrt(nx**2+ny**2),nx/math.sqrt(nx**2+ny**2) 
								n+=[ [ [x1,y1], [nx,ny], False, False, i] ] # [point coordinates, normal, is an inner corner, is an outer corner, csp's index]
								if ti==1 and i<len(csp)-1:
									bez1 = (csp[i][1][:],csp[i][2][:],csp[i+1][0][:],csp[i+1][1][:])
									nx2, ny2 = bezmisc.bezierslopeatt(bez1,0)
									nx2,ny2 = -ny2/math.sqrt(nx2**2+ny2**2),nx2/math.sqrt(nx2**2+ny2**2) 
									ang = ny2*nx-ny*nx2
									ang1 = 180-math.acos(max(-1,min(1,nx*nx2+ny*ny2)))*180/math.pi
		 							if ang > 0  and ang1 < self.options.engraving_sharp_angle_tollerance :	# inner angle
										n[-1][2] = True
		 							elif ang < 0 and ang1 < self.options.engraving_sharp_angle_tollerance :					# outer angle
		 								a = -math.acos(nx*nx2+ny*ny2)
		 								for t in [.0,.25,.75,1.]:
		 									n1 += [ [ [x1,y1], [nx*math.cos(a*t)-ny*math.sin(a*t),nx*math.sin(a*t)+ny*math.cos(a*t)], False, True, i ]  ]
			 				nl += [ n ] + ([ n1 ] if n1!=[] else [])
			 			# Modify first/last points if curve is closed
						if abs(csp[-1][1][0]-csp[0][1][0])<engraving_tolerance and abs(csp[-1][1][1]-csp[0][1][1])<engraving_tolerance :
							bez1 = (csp[-2][1][:],csp[-2][2][:],csp[-1][0][:],csp[-1][1][:])
							x1,y1 = bezmisc.bezierpointatt(bez1,1)
							nx,ny = bezmisc.bezierslopeatt(bez1,1)
							nx,ny = -ny/math.sqrt(nx**2+ny**2),nx/math.sqrt(nx**2+ny**2) 
							bez = (csp[0][1][:],csp[0][2][:],csp[1][0][:],csp[1][1][:])
							nx2,ny2 = bezmisc.bezierslopeatt(bez,0)
							nx2,ny2 = -ny2/math.sqrt(nx2**2+ny2**2),nx2/math.sqrt(nx2**2+ny2**2)
							ang = ny2*nx-ny*nx2
							if ang > 0  and 180-math.acos(nx*nx2+ny*ny2)*180/math.pi < self.options.engraving_sharp_angle_tollerance :	# inner angle
								nl[-1][-1][2] = True
 							elif ang < 0 and 180-math.acos(nx*nx2+ny*ny2)*180/math.pi < self.options.engraving_sharp_angle_tollerance :					# outer angle
 								a = -math.acos(nx*nx2+ny*ny2)
					 			n1 = []
 								for t in [.0,.25,.75,1.]:
 									n1 += [ [ [x1,y1], [nx*math.cos(a*t)-ny*math.sin(a*t),nx*math.sin(a*t)+ny*math.cos(a*t)], False, True, i ]  ]
				 				nl += [ n1 ] 



						if self.options.engraving_draw_calculation_paths==True:
							for i in nl:
								for p in i:
									inkex.etree.SubElement(	self.Group, inkex.addNS('path','svg'), 
										{
											 "d":	"M %f,%f L %f,%f" %(p[0][0],p[0][1],p[0][0]+p[1][0]*10,p[0][1]+p[1][1]*10),
											'style':	"stroke:#0000ff; stroke-opacity:0.46; stroke-width:0.1; fill:none",
										})				


							
			 			# 	Calculate offset points	
			 			csp_points = [] 
						for ki in xrange(len(nl)):
							p = []
							for ti in xrange(3) if ki!=len(nl)-1 else xrange(4):
								n = nl[ki][ti]
								x1,y1 = n[0]
								nx,ny = n[1]
								d, r = 0, None
								if ti==0 and nl[ki-1][-1][2] == True 	or 		ti==3 and nl[ki][ti][2] == True:
									# Point is a sharp angle r=0p
									r = 0
								else :
									for j in xrange(0,len(cspi)):
										for i in xrange(1,len(cspi[j])):
											d = point_to_csp_bound_dist([x1,y1], cspi[j][i-1], cspi[j][i], self.options.engraving_max_dist*2)
											if d>=self.options.engraving_max_dist*2 :
												r = min(d/2,r) if r!=None else d/2	
												continue
											for n1 in xrange(self.options.engraving_newton_iterations):
						 						t = find_cutter_center((x1,y1),(nx,ny), cspi[j][i-1], cspi[j][i], float(n1)/(self.options.engraving_newton_iterations-1))
												if t[0] > engraving_tolerance and 0<=t[2]<=1 and abs(t[3])<engraving_tolerance:
													t3 = t[2]
													ax,ay,bx,by,cx,cy,dx,dy=bezmisc.bezierparameterize((cspi[j][i-1][1],cspi[j][i-1][2],cspi[j][i][0],cspi[j][i][1]))
													x2=ax*(t3*t3*t3)+bx*(t3*t3)+cx*t3+dx
													y2=ay*(t3*t3*t3)+by*(t3*t3)+cy*t3+dy
												
													if abs(x2-x1)<engraving_tolerance and abs(y2-y1)<engraving_tolerance:
														f1x = 3*ax*(t3*t3)+2*bx*t3+cx
														f1y = 3*ay*(t3*t3)+2*by*t3+cy
														f2x = 6*ax*t3+2*bx
														f2y = 6*ay*t3+2*by
														d = f1x*f2y-f1y*f2x
														if d!=0 :
															d = math.sqrt((f1x*f1x+f1y*f1y)**3)/d
															if d>0:
																r = min( d,r) if r!=None else d
															else :
																r = min(r,self.options.engraving_max_dist) if r!=None else self.options.engraving_max_dist
													else:						
							 							r = min(t[0],r) if r!=None else t[0]	
									for j in xrange(0,len(cspi)):
										for i in xrange(0,len(cspi[j])):
											x2,y2 = cspi[j][i][1]
											if (abs(x1-x2)>engraving_tolerance or abs(y1-y2)>engraving_tolerance ) and (x2*nx - x1*nx + y2*ny - y1*ny) != 0:
												t1 = .5 * ( (x1-x2)**2+(y1-y2)**2 ) /  (x2*nx - x1*nx + y2*ny - y1*ny)
												if t1>0 : r = min(t1,r) if r!=None else t1
								if self.options.engraving_draw_calculation_paths==True:
									inkex.etree.SubElement(	self.Group, inkex.addNS('path','svg'), 
											{'style':	"fill:#ff00ff; fill-opacity:0.46; stroke:#000000; stroke-width:0.1;", inkex.addNS('cx','sodipodi'):		str(x1+nx*r), inkex.addNS('cy','sodipodi'):		str(y1+ny*r), inkex.addNS('rx','sodipodi'):	str(1), inkex.addNS('ry','sodipodi'): str(1), inkex.addNS('type','sodipodi'):	'arc'})	
									inkex.etree.SubElement(	self.Group, inkex.addNS('path','svg'), 
											{'style':	"fill:none; fill-opacity:0.46; stroke:#000000; stroke-width:0.1;", inkex.addNS('cx','sodipodi'):		str(x1+nx*r),  inkex.addNS('cy','sodipodi'):		str(y1+ny*r),inkex.addNS('rx','sodipodi'):		str(r), inkex.addNS('ry','sodipodi'):		str(r), inkex.addNS('type','sodipodi'):	'arc'})

								r = min(r, self.options.engraving_max_dist)
								w = min(r, self.options.tool_diameter)
								p += [ [x1+nx*w,y1+ny*w,r,w] ]
								
							
								
							if len(csp_points)>0 : csp_points[-1] += [p[0]]						 			
							csp_points += [ p ]
						#	Splitting path to pieces each of them not further from path more than engraving_max_dist
						engraving_path = [ [] ]
						for p_ in csp_points :
							for p in p_:
								if p[2]<self.options.engraving_max_dist : break
							if p[2]<self.options.engraving_max_dist: engraving_path[-1] += [p_]
							else : 
								if engraving_path[-1] != [] : engraving_path += [ [] ]
						if engraving_path[-1] == [] : del engraving_path[-1] 
						
						
						for csp_points in engraving_path :
							#	Create Path that goes through this points 
							cspm = []
							w = []
							m = [[0.0, 0.0, 0.0, 1.0], [0.015625, 0.140625, 0.421875, 0.421875], [0.421875, 0.421875, 0.140625, 0.015625], [1.0, 0.0, 0.0, 0.0]]
							for p in csp_points:
								m = numpy.array(m)
								xi = numpy.array( [p[i][:2] for i in range(4)])
								sp1,sp2 = [[0.,0.],[0.,0.],[0.,0.]], [[0.,0.],[0.,0.],[0.,0.]]
								a,b,c,d = numpy.linalg.solve(m, xi).tolist()
								sp1[1], sp1[0] = d, d
								sp1[2] = c
								sp2[0] = b
								sp2[1], sp2[2] = a, a
								sp3,sp4,sp5 = cspbezsplit(sp1, sp2, .25)
								l = cspseglength(sp3,sp4)
								sp1,sp2,sp4 = cspbezsplit(sp1, sp2, .75)
								l1 = cspseglength(sp1,sp2)
								if l1!=0:
									sp1,sp2,sp3 = cspbezsplitatlength(sp1, sp2, l/l1)
									if len(cspm)>0 :
										cspm[-1][2] = sp1[2]
										cspm += [sp2[:], sp3[:], sp4[:]]
										w +=  [p[i][3] for i in range(1,4)] 
									else :
										cspm += [sp1[:], sp2[:], sp3[:], sp4[:]]	
										w += [p[i][3] for i in range(4)]

							node =  inkex.etree.SubElement(	self.Group, inkex.addNS('path','svg'), 										{
														 "d":	 cubicsuperpath.formatPath([cspm]),
														'style':				biarc_style_i['biarc1']
													})
							for i in xrange(len(cspm)):
								inkex.etree.SubElement(	self.Group, inkex.addNS('path','svg'), 
											{'style':	"fill:none; fill-opacity:0.46; stroke:#000000; stroke-width:0.1;", inkex.addNS('cx','sodipodi'):		str(cspm[i][1][0]),  inkex.addNS('cy','sodipodi'):		str(cspm[i][1][1]),inkex.addNS('rx','sodipodi'):		str(w[i]), inkex.addNS('ry','sodipodi'):		str(w[i]), inkex.addNS('type','sodipodi'):	'arc'})

							cspe += [cspm]
							we   +=	[w]				

			if self.options.engraving_cutter_shape_function != "":
				f = eval('lambda w: ' + self.options.engraving_cutter_shape_function.strip('"'))
			else: f = lambda w: w
			if cspe!=[]:
				curve = self.parse_curve(cspe,we,f)
				self.draw_curve(curve,self.Group)
				gcode += self.generate_gcode(curve,self.options.Zsurface)
				gcode += self.footer
				try: 	
					f = open(self.options.directory+'/'+self.options.file, "w")	
					f.write(gcode)
					f.close()							
				except:
					inkex.errormsg(_("Can not write to specified file!"))
					return
			else : 	inkex.errormsg(_("No need to engrave sharp angles."))
	

################################################################################
###
###		Orientation
###
################################################################################

		elif self.options.active_tab == '"orientation"' :
			points = [[self.options.orientation_point1x, self.options.orientation_point1y], [self.options.orientation_point2x, self.options.orientation_point2y], [self.options.orientation_point3x, self.options.orientation_point3y]]
			orientation_group = inkex.etree.SubElement(self.current_layer if self.current_layer is not None else self.document.getroot(), inkex.addNS('g','svg'))
			for i in points :
				si = [i[0]*self.options.orientation_scale, i[1]*self.options.orientation_scale]
				g = inkex.etree.SubElement(orientation_group, inkex.addNS('g','svg'), {'comment': "Gcode tools orientation point"})
				inkex.etree.SubElement(	g, inkex.addNS('path','svg'), 
					{
						'style':	"stroke:none;fill:#000000;", 	
						'd':'m %s,%s 2.9375,-6.343750000001 0.8125,1.90625 6.843748640396,-6.84374864039 0,0 0.6875,0.6875 -6.84375,6.84375 1.90625,0.812500000001 z z' % (si[0], -si[1]),
						'comment': "Gcode tools orientation point arrow"
					})
				t = inkex.etree.SubElement(	g, inkex.addNS('text','svg'), 
					{
						'style':	"font-size:10px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;fill:#000000;fill-opacity:1;stroke:none;",
						'x':	str(si[0]+10),
						'y':	str(-si[1]-10),
						'comment': "Gcode tools orientation point text"
					})
				t.text = "(%s; %s)" % (i[0],i[1])
				
		
		
							
e = Gcode_tools()
e.affect()