#!/usr/bin/python
#    planar spline to arc conversion
#    Copyright 2007 Jeff Epler <jepler@unpythonic.net>
#
#    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

import math

class P:
    def __init__(self, x, y):
	self.x, self.y = x, y

    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 __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 "<%.2f %.2f>" % (self.x, self.y)

def biarc(P0, TS, P4, TE, r):
    TS = TS.unit()
    TE = TE.unit()

    v = P0 - P4

    c = v*v
    b = 2*v*(r*TS+TE)
    a = 2*r*(TS*TE-1)

    if a == 0:
	raise ValueError, (a,b,c)
    
    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
    #print (beta1, beta2)
    if beta1 > 0 and beta2 > 0:
	raise ValueError, (a,b,c,disq,beta1,beta2)
    beta = max(beta1, beta2)

    if beta < 0:
	raise ValueError, (a,b,c,disq,beta)

    alpha = beta * r
    ab = alpha + beta 
    P1 = P0 + alpha * TS
    P3 = P4 - beta * TE
    P2 = (beta / ab)  * P1 + (alpha / ab) * P3

    #print "(%f %f %f %f)" % (a, b, c, discr)
    #print "(%f %f %f %f)" % (P2.x, P2.y, alpha, beta)
    return P1, P2, P3, alpha, beta

small = 1e-10
def unit(a,b):
    h = math.hypot(a, b)
    if h: return a/h, b/h
    else: return a, b

def arc(x1, y1, x2, y2, dx, dy):
    """\
r(x1,y1, x2,y2, dx,dy) -> dx, dy

Given the initial location (x1,y1) the desired location
(x2,y2), and the direction of travel at the initial location
(dx,dy), find the line or arc satisfying these conditions
and output a line of g-code to perform that arc

In the case that the target point is exactly in the opposite
direction of the initial direction of motion, the tangency
condition cannot be maintained.  In this case, the points
are joined with a line that violates the tangency condition.

Returns the direction of travel at the final location
"""
    dx, dy = unit(dx, dy)
    #print "(r %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f)" % (x1, y1, x2, y2, dx, dy),

    x = x2-x1
    y = y2-y1

    den = 2*(y*dx - x*dy)

    if abs(den) < small:  # Line
        print "G1 X%.4f Y%.4f" % (x2, y2)
        dx = x2-x1; dy = y2-y1
        u = math.hypot(dx, dy)
        return unit(x2-x1, y2-y1)

    r = -(x**2 + y**2) / den

    i = dy * r
    j = -dx * r
    cx = x1 + i
    cy = y1 + j

    if r < 0:   # Counterclockwise
        print "G3 I%.4f J%.4f X%.4f Y%.4f" % (i, j, x2, y2)
        return unit(-(y2-cy), x2-cx)
    else:       # Clockwise
        print "G2 I%.4f J%.4f X%.4f Y%.4f" % (i, j, x2, y2)
        return unit(y2-cy, -(x2-cx))

def giarc(PS, TS, PE, TE, r=1):
    TS = TS.unit()
    TE = TE.unit()
    P1, P2, P3, alpha, beta = biarc(PS, TS, PE, TE, r)

    if 0:
	print "G0 X%.4f Y%.4f" % (P1.x, P1.y)
	print "G0 X%.4f Y%.4f" % (P2.x, P2.y)
	print "G4 P0"
	print "G0 X%.4f Y%.4f" % (P3.x, P3.y)
	print "G0 X%.4f Y%.4f" % (PE.x, PE.y)
	print "G0 X%.4f Y%.4f" % (PS.x, PS.y)

    arc(PS.x, PS.y, P2.x, P2.y, TS.x, TS.y)
    arc(P2.x, P2.y, PE.x, PE.y, P3.x-P2.x, P3.y-P2.y)

def spline(P0, P1, P2, P3, n=5):
    def _spline(t):
	return P0*(1-t)**3 + 3*P1*t*(1-t)**2 + 3*P2*t**2*(1-t) + P3*t**3
    def _tan(t):
	return Q0*(1-t)**2 + 2*Q1*t*(1-t) + Q2*t**2
    Q0 = 3*(P1-P0)
    Q1 = 3*(P2-P1)
    Q2 = 3*(P3-P2)

    PS = P0
    TS = P1 - P0
    for i in range(0, n):
	t1 = (i + 1) * 1. / n
	PE = _spline(t1)
	TE = _tan(t1)
	
	giarc(PS, TS, PE, TE)

	PS = PE
	TS = TE

if __name__ == '__main__':
    print "G0X0Y0Z0"
    print "F100"
    spline(P(0,0), P(.25,1), P(1,1), P(2,0), 2)
    print "G0X0Y0Z0"
    spline(P(0,0), P(.25,1), P(1,1), P(2,0), 4)
    print "G0X0Y0Z0"
    spline(P(0,0), P(.25,1), P(1,1), P(2,0), 8)
    print "M2"