#!/usr/bin/python

import pygtk
pygtk.require("2.0")
import gtk
import re
import sys
import os
from math import sqrt, cos, sin, acos, asin, pi, copysign, atan2, degrees

MULTIPLIFER = 5

DRAW_ORIGINAL = False

OFFSET = 0.45
PRECISION = 0.05

STEP = acos( 1-( (PRECISION*PRECISION)/(2*OFFSET*OFFSET) ) )

#test_file = 'lyx_130102_oracal_06.tap'
#test_file = '5.tap'

if len(sys.argv)>1:
	test_file = sys.argv[1]
	out_file = '%s\\fluged_%s' % (os.path.dirname(sys.argv[1]), os.path.basename(sys.argv[1]))
	#print out_file
	#raw_input()
else:
	sys.exit(0)

g_codes = [ i.strip( chr(13)+chr(10) ) for i in  open(test_file).readlines() ]

re_parce_commands = re.compile(r'[GMTXYZFPFSRDIJKL][\-\d\.]+', re.M)

def line_len(a, b):
	return sqrt( pow((a[1]-b[1]),2) + pow((a[0]-b[0]),2) )

def get_line_offset(a, b, offset, external = True):
	l = line_len(a, b)
	dx = offset*(a[0]-b[0])/l
	dy = offset*(a[1]-b[1])/l
	list_cx = [b[0]+dx, b[0]-dx]
	list_cy = [b[1]+dy, b[1]-dy]
	if external:
		max_lc = 0
	else:
		min_lc = 65535*65535
	for cx in list_cx:
		for cy in list_cy:			
			lc = line_len(a, (cx, cy) )
			if external:
				if lc > max_lc:
					rcx = cx
					rcy = cy
					max_lc = lc
			else:
				if lc < min_lc:
					rcx = cx
					rcy = cy
					min_lc = lc
	return rcx, rcy

def get_direction_angle(a1,a2):
	na1 = min(a1,a2)
	na2 = max(a1,a2)
	da1 = na2-na1
	da2 = 2*pi+na1-na2
	return min(da1, da2), (da1 > da2) != (a1 != na1)
	
def get_line_deg(a,b):
	return atan2( b[1]-a[1], b[0]-a[0] )
	
def correct_cmd(cmds, new_pos, old_pos):
	correct = []
	if round(new_pos[0],3) != round(old_pos[0],3):
		correct.append( 'X%.3f' % round(new_pos[0],3) )
	if round(new_pos[1],3) != round(old_pos[1],3):
		correct.append( 'Y%.3f' % round(new_pos[1],3) )
	if cmds[0] == 'G1' or cmds[0] == 'G0':
		return [cmds[0]] + correct + [ i for i in cmds[1:] if i[0] not in 'XY' ]
	elif cmds[0][0] in 'XYZ':
		return correct + [ i for i in cmds[1:] if i[0] not in 'XY' ]
	else:
		raise Exception('Correct problem, please fix for cmd %s' % cmds)
	

def fluged_codes(codes, self = None):
	pos = [0,0]
	paint = False
	pos_changed = False
	pos_buf = []	
	out = []
	bypass = False
	for i in codes:
		cmds = re_parce_commands.findall(i)
		if i != ''.join(cmds):
			print i, '-' , ''.join(cmds)
			raise Exception('Lost G-code')
		pos_changed = False
		if cmds[0] == 'G0' or cmds[0] == 'G1' or cmds[0][0] in 'XYZ':
			for i in cmds:
				if i[0] == 'X':
					pos[0] = float(i[1:])
					pos_changed = True
				if i[0] == 'Y':
					pos[1] = float(i[1:])
					pos_changed = True
			if pos_changed:
				new_pos = None
				if cmds[0] == 'G0': paint = False
				if cmds[0] == 'G1': paint = True
				if len(pos_buf)>0:
					pos_changed = round(pos[0],3) != round(pos_buf[-1][1][0],3) or round(pos[1],3) != round(pos_buf[-1][1][1],3)
				if pos_changed:
					if paint:
						if self and DRAW_ORIGINAL: self.line_to(*pos)
						if not pos_buf[-1][0]:
							new_pos = get_line_offset(pos_buf[-1][1], pos, OFFSET)
							cut_correct = get_line_offset(pos, pos_buf[-1][1], OFFSET)
							if self and not DRAW_ORIGINAL: self.line_to(*new_pos)
							if out[-1][0] == 'G1' and out[-1][1][0] == 'Z':
								if out[-2][0] == 'G0':
									out[-2] = correct_cmd(out[-2], cut_correct, pos_buf[-2][1])
									pos_buf[-1][1] = cut_correct
								else:
									Exception('New start codes, not found G0, please fix')
							else:
								raise Exception('New start codes, not found G1 or Z, please fix')
                                                                
							cmds = correct_cmd(cmds, new_pos, pos_buf[-1][1])
						else:
							new_pos = get_line_offset(pos_buf[-1][1], pos, OFFSET)
							int_cb = get_line_offset(pos, pos_buf[-1][1], OFFSET, False)
							deg_ab = get_line_deg( pos_buf[-2][1], pos_buf[-1][1] )
							deg_bc = get_line_deg( pos_buf[-1][1], pos )
							angle, direction = get_direction_angle(deg_ab, deg_bc)
							if direction:
								deg = deg_ab-angle
							else:
								deg = deg_ab+angle
							cord = ( pos_buf[-1][1][0]+OFFSET*cos(deg), pos_buf[-1][1][1]+OFFSET*sin(deg) )
							for_mod = False
							for i in range(int(angle/STEP)):
								for_mod = True
								if direction:
									deg = deg_ab-STEP*(i+1)
								else:
									deg = deg_ab+STEP*(i+1)
								cord = ( pos_buf[-1][1][0]+OFFSET*cos(deg), pos_buf[-1][1][1]+OFFSET*sin(deg) )
								if self and not DRAW_ORIGINAL: self.line_to(*cord)
								out.append( [ 'X%.3f' % round(cord[0],3), 'Y%.3f' % round(cord[1],3) ] )
							if self and not DRAW_ORIGINAL: self.line_to(*int_cb)
							out.append( [ 'X%.3f' % round(int_cb[0],3), 'Y%.3f' % round(int_cb[1],3) ] )
							if self and not DRAW_ORIGINAL: self.line_to(*new_pos)							
							if for_mod: cmds = correct_cmd(cmds, new_pos, cord)
							if self and DRAW_ORIGINAL: self.line_to(*pos)
					else:
						if self: self.move_to(*pos)
				if pos_changed:
					pos_buf.append( [paint, pos[:], new_pos] )
		if not bypass:
			out.append(cmds)
	f = open(out_file,'wb')
	for i in out:
		f.write( ''.join(i)+'\n' )

class Area(gtk.DrawingArea):
	def __init__(self):
		gtk.DrawingArea.__init__(self)
		self.connect("expose_event", self.expose)
		
	def expose(self, widget, event):
		self.context = widget.window.cairo_create()
		
		# set a clip region for the expose event
		self.context.rectangle(event.area.x, event.area.y,
							   event.area.width, event.area.height)
		self.context.clip()
		
		self.draw(self.context)
		
		return False
		
	def line_to(self, x, y):
		self.context.line_to(x * MULTIPLIFER, self.rect[3] - y * MULTIPLIFER)

	def move_to(self, x, y):
		self.context.move_to(x * MULTIPLIFER, self.rect[3] - y * MULTIPLIFER)
	
	def draw(self, context):
		rect = self.get_allocation()
		self.context = context
		self.rect = rect
		context.set_source_rgb(0, 0, 0)
		fluged_codes(g_codes, self)
		context.stroke()

def main1():
	window = gtk.Window()
	window.resize(700,400)
	area = Area()
	
	window.add(area)
	window.connect("destroy", gtk.main_quit)
	window.show_all()
	
	gtk.main()
	
def main2():
	fluged_codes(g_codes)
	
if __name__ == "__main__":
	main1()