import sys
from math import *

from Numeric import *
import LinearAlgebra as la

import poly3

n = 15
m = zeros(((n + 1) * 4, (n + 1) * 4), Float)
for i in range(n):
    m[4 * i + 2][4 * i + 0] = .5
    m[4 * i + 2][4 * i + 1] = -1./12
    m[4 * i + 2][4 * i + 2] = 1./48
    m[4 * i + 2][4 * i + 3] = -1./480
    m[4 * i + 2][4 * i + 4] = .5
    m[4 * i + 2][4 * i + 5] = 1./12
    m[4 * i + 2][4 * i + 6] = 1./48
    m[4 * i + 2][4 * i + 7] = 1./480

    m[4 * i + 3][4 * i + 0] = 1
    m[4 * i + 3][4 * i + 1] = .5
    m[4 * i + 3][4 * i + 2] = .125
    m[4 * i + 3][4 * i + 3] = 1./48
    m[4 * i + 3][4 * i + 4] = -1
    m[4 * i + 3][4 * i + 5] = .5
    m[4 * i + 3][4 * i + 6] = -.125
    m[4 * i + 3][4 * i + 7] = 1./48

    m[4 * i + 4][4 * i + 0] = 0
    m[4 * i + 4][4 * i + 1] = 1
    m[4 * i + 4][4 * i + 2] = .5
    m[4 * i + 4][4 * i + 3] = .125
    m[4 * i + 4][4 * i + 4] = 0
    m[4 * i + 4][4 * i + 5] = -1
    m[4 * i + 4][4 * i + 6] = .5
    m[4 * i + 4][4 * i + 7] = -.125

    m[4 * i + 5][4 * i + 0] = 0
    m[4 * i + 5][4 * i + 1] = 0
    m[4 * i + 5][4 * i + 2] = 1
    m[4 * i + 5][4 * i + 3] = .5
    m[4 * i + 5][4 * i + 4] = 0
    m[4 * i + 5][4 * i + 5] = 0
    m[4 * i + 5][4 * i + 6] = -1
    m[4 * i + 5][4 * i + 7] = .5

m[n * 4 + 2][2] = 1
m[n * 4 + 3][3] = 1

m[0][n * 4 + 2] = 1
m[1][n * 4 + 3] = 1

def printarr(m):
    for j in range(n * 4 + 4):
        for i in range(n * 4 + 4):
            print '%6.1f' % m[j][i],
        print ''

sys.output_line_width = 160
#print array2string(m, precision = 3)
mi = la.inverse(m)
#printarr(mi)

# fit arc to pts (0, 0), (x, y), and (1, 0), return th tangent to
# arc at (x, y)
def fit_arc(x, y):
    th = atan2(y - 2 * x * y, y * y + x - x * x)
    return th

def mod_2pi(th):
    u = th / (2 * pi)
    return 2 * pi * (u - floor(u + 0.5))

def plot_path(path, th, k):
    if path[0][2] == '{': closed = 0
    else: closed = 1
    j = 0
    cmd = 'moveto'
    for i in range(len(path) + closed - 1):
        i1 = (i + 1) % len(path)
        x0, y0, t0 = path[i]
        x1, y1, t1 = path[i1]
        j1 = (j + 1) % len(th)
        th0 = th[j]
        k0 = k[j]
        th1 = th[j1]
        k1 = k[j1]
        chord_th = atan2(y1 - y0, x1 - x0)
        chord_len = hypot(y1 - y0, x1 - x0)
        ks = poly3.solve_3spiro(mod_2pi(chord_th - th0),
                                mod_2pi(th1 - chord_th),
                                k0 * chord_len, k1 * chord_len)
        ksp = (ks[0] * .5, ks[1] * .25, ks[2] * .125, ks[3] * .0625)
        pside = poly3.int_3spiro_poly(ksp, 64)
        ksm = (ks[0] * -.5, ks[1] * .25, ks[2] * -.125, ks[3] * .0625)
        mside = poly3.int_3spiro_poly(ksm, 64)
        mside.reverse()
        for i in range(len(mside)):
            mside[i] = (-mside[i][0], -mside[i][1])
        pts = mside + pside[1:]
        rot = chord_th - atan2(pts[-1][1] - pts[0][1], pts[-1][0] - pts[0][0])
        scale = hypot(x1 - x0, y1 - y0) / hypot(pts[-1][1] - pts[0][1], pts[-1][0] - pts[0][0])
        u, v = scale * cos(rot), scale * sin(rot)
        xt = x0 - u * pts[0][0] + v * pts[0][1]
        yt = y0 - u * pts[0][1] - v * pts[0][0]
        for x, y in pts:
            print xt + u * x - v * y, yt + u * y + v * x, cmd
            cmd = 'lineto'
        if t1 == 'v':
            j += 2
        else:
            j += 1
    print 'stroke'
    if 0:
        j = 0
        for i in range(len(path)):
            x0, y0, t0 = path[i]
            print 'gsave', x0, y0, 'translate'
            print ' ', th[j] * 180 / pi, 'rotate'
            print '  -50 0 moveto 50 0 lineto stroke'
            print 'grestore'
            j += 1

def plot_ks(path, th, k):
    if path[0][2] == '{': closed = 0
    else: closed = 1
    j = 0
    cmd = 'moveto'
    xo = 36
    yo = 550
    xscale = .5
    yscale = 2000
    x = xo
    for i in range(len(path) + closed - 1):
        i1 = (i + 1) % len(path)
        x0, y0, t0 = path[i]
        x1, y1, t1 = path[i1]
        j1 = (j + 1) % len(th)
        th0 = th[j]
        k0 = k[j]
        th1 = th[j1]
        k1 = k[j1]
        chord_th = atan2(y1 - y0, x1 - x0)
        chord_len = hypot(y1 - y0, x1 - x0)
        ks = poly3.solve_3spiro(mod_2pi(chord_th - th0),
                                mod_2pi(th1 - chord_th),
                                k0 * chord_len, k1 * chord_len)
        ksp = (ks[0] * .5, ks[1] * .25, ks[2] * .125, ks[3] * .0625)
        pside = poly3.int_3spiro_poly(ksp, 64)
        ksm = (ks[0] * -.5, ks[1] * .25, ks[2] * -.125, ks[3] * .0625)
        mside = poly3.int_3spiro_poly(ksm, 64)
        print '%', x0, y0, k0, k1
        print "% ks = ", ks
        l = 2 * chord_len / hypot(pside[-1][0] + mside[-1][0], pside[-1][1] + mside[-1][1])
        for i in range(100):
            s = .01 * i - 0.5
            kp = poly3.eval_cubic(ks[0], ks[1], .5 * ks[2], 1./6 * ks[3], s)
            kp /= l
            print x + xscale * l * (s + .5), yo + yscale * kp, cmd
            cmd = 'lineto'
        x += xscale * l
        if t1 == 'v':
            j += 2
        else:
            j += 1
    print 'stroke'
    print xo, yo, 'moveto', x, yo, 'lineto stroke'

def make_error_vec(path, th, k):
    if path[0][2] == '{': closed = 0
    else: closed = 1
    n = len(path)
    v = zeros(n * 2, Float)
    j = 0
    for i in range(len(path) + closed - 1):
        i1 = (i + 1) % len(path)
        x0, y0, t0 = path[i]
        x1, y1, t1 = path[i1]
        j1 = (j + 1) % len(th)
        th0 = th[j]
        k0 = k[j]
        th1 = th[j1]
        k1 = k[j1]
        chord_th = atan2(y1 - y0, x1 - x0)
        chord_len = hypot(y1 - y0, x1 - x0)
        ks = poly3.solve_3spiro(mod_2pi(chord_th - th0),
                                mod_2pi(th1 - chord_th),
                                k0 * chord_len, k1 * chord_len)
        ksp = (ks[0] * .5, ks[1] * .25, ks[2] * .125, ks[3] * .0625)
        pside = poly3.int_3spiro_poly(ksp, 64)
        ksm = (ks[0] * -.5, ks[1] * .25, ks[2] * -.125, ks[3] * .0625)
        mside = poly3.int_3spiro_poly(ksm, 64)
        l = chord_len / hypot(pside[-1][0] + mside[-1][0], pside[-1][1] + mside[-1][1])
        k1l = (ks[1] - .5 * ks[2] + .125 * ks[3]) / (l * l)
        k1r = (ks[1] + .5 * ks[2] + .125 * ks[3]) / (l * l)
        k2l = (ks[2] - .5 * ks[3]) / (l * l * l)
        k2r = (ks[2] + .5 * ks[3]) / (l * l * l)

        if t0 == 'o' or t0 == '[' or t0 == 'v':
            v[2 * j] -= k1l
            v[2 * j + 1] -= k2l
        elif t0 == 'c':
            v[2 * j + 1] += k2l

        if t1 == 'o' or t1 == ']' or t1 == 'v':
            v[2 * j1] += k1r
            v[2 * j1 + 1] += k2r
        elif t1 == 'c':
            v[2 * j1] += k2r

        print "% left k'", k1l, "k''", k2l, "right k'", k1r, "k''", k2r
        if t1 == 'v':
            j += 2
        else:
            j += 1
    print '% error vector:'
    for i in range(n):
        print '%  ', v[2 * i], v[2 * i + 1]
    return v

def add_k1l(m, row, col, col1, l, s):
    l2 = l * l
    m[row][2 * col] += s * 36 / l2
    m[row][2 * col + 1] -= s * 9 / l
    m[row][2 * col1] += s * 24 / l2
    m[row][2 * col1 + 1] += s * 3 / l

def add_k1r(m, row, col, col1, l, s):
    l2 = l * l
    m[row][2 * col] += s * 24 / l2
    m[row][2 * col + 1] -= s * 3 / l
    m[row][2 * col1] += s * 36 / l2
    m[row][2 * col1 + 1] += s * 9 / l

def add_k2l(m, row, col, col1, l, s):
    l2 = l * l
    l3 = l2 * l
    m[row][2 * col] -= s * 192 / l3
    m[row][2 * col + 1] += s * 36 / l2
    m[row][2 * col1] -= s * 168 / l3
    m[row][2 * col1 + 1] -= s * 24 / l2

def add_k2r(m, row, col, col1, l, s):
    l2 = l * l
    l3 = l2 * l
    m[row][2 * col] += s * 168 / l3
    m[row][2 * col + 1] -= s * 24 / l2
    m[row][2 * col1] += s * 192 / l3
    m[row][2 * col1 + 1] += s * 36 / l2

def make_matrix(path, th, k):
    if path[0][2] == '{': closed = 0
    else: closed = 1
    n = len(path)
    m = zeros((n * 2, n * 2), Float)
    j = 0
    for i in range(len(path) + closed - 1):
        i1 = (i + 1) % len(path)
        x0, y0, t0 = path[i]
        x1, y1, t1 = path[i1]
        j1 = (j + 1) % len(th)
        th0 = th[j]
        k0 = k[j]
        th1 = th[j1]
        k1 = k[j1]
        chord_th = atan2(y1 - y0, x1 - x0)
        chord_len = hypot(y1 - y0, x1 - x0)
        ks = poly3.solve_3spiro(mod_2pi(chord_th - th0),
                                mod_2pi(th1 - chord_th),
                                k0 * chord_len, k1 * chord_len)
        ksp = (ks[0] * .5, ks[1] * .25, ks[2] * .125, ks[3] * .0625)
        pside = poly3.int_3spiro_poly(ksp, 64)
        ksm = (ks[0] * -.5, ks[1] * .25, ks[2] * -.125, ks[3] * .0625)
        mside = poly3.int_3spiro_poly(ksm, 64)
        l = chord_len / hypot(pside[-1][0] + mside[-1][0], pside[-1][1] + mside[-1][1])

        if t0 == 'o' or t0 == '[' or t0 == 'v':
            add_k1l(m, 2 * j, j, j1, l, -1)
            add_k2l(m, 2 * j + 1, j, j1, l, -1)
        elif t0 == 'c':
            add_k2l(m, 2 * j + 1, j, j1, l, 1)

        if t1 == 'o' or t1 == ']' or t1 == 'v':
            add_k1r(m, 2 * j1, j, j1, l, 1)
            add_k2r(m, 2 * j1 + 1, j, j1, l, 1)
        elif t1 == 'c':
            add_k2r(m, 2 * j1, j, j1, l, 1)

        if t1 == 'v':
            j += 2
        else:
            j += 1
    return m

def solve(path):
    if path[0][2] == '{': closed = 0
    else: closed = 1
    dxs = []
    dys = []
    chords = []
    for i in range(len(path) - 1):
        dxs.append(path[i + 1][0] - path[i][0])
        dys.append(path[i + 1][1] - path[i][1])
        chords.append(hypot(dxs[-1], dys[-1]))
    nominal_th = []
    nominal_k = []
    if not closed:
        nominal_th.append(atan2(dys[0], dxs[0]))
        nominal_k.append(0)
    for i in range(1 - closed, len(path) - 1 + closed):
        x0, y0, t0 = path[(i + len(path) - 1) % len(path)]
        x1, y1, t1 = path[i]
        x2, y2, t2 = path[(i + 1) % len(path)]
        dx = float(x2 - x0)
        dy = float(y2 - y0)
        ir2 = dx * dx + dy * dy
        x = ((x1 - x0) * dx + (y1 - y0) * dy) / ir2
        y = ((y1 - y0) * dx - (x1 - x0) * dy) / ir2
        th = fit_arc(x, y) + atan2(dy, dx)
        bend_angle = mod_2pi(atan2(y2 - y1, x2 - x1) - atan2(y1 - y0, x1 - x0))
        k = 2 * bend_angle/(hypot(y2 - y1, x2 - x1) + hypot(y1 - y0, x1 - x0))
        print '% bend angle', bend_angle, 'k', k
        if t1 == ']':
            th = atan2(y1 - y0, x1 - x0)
            k = 0
        elif t1 == '[':
            th = atan2(y2 - y1, x2 - x1)
            k = 0
        nominal_th.append(th)
        nominal_k.append(k)
    if not closed:
        nominal_th.append(atan2(dys[-1], dxs[-1]))
        nominal_k.append(0)
    print '%', nominal_th
    print '0 0 1 setrgbcolor .5 setlinewidth'
    plot_path(path, nominal_th, nominal_k)
    plot_ks(path, nominal_th, nominal_k)
    th = nominal_th[:]
    k = nominal_k[:]
    n = 8
    for i in range(n):
        ev = make_error_vec(path, th, k)
        m = make_matrix(path, th, k)
        #print m
        #print 'inverse:'
        #print la.inverse(m)
        v = dot(la.inverse(m), ev)
        #print v
        for j in range(len(path)):
            th[j] += 1. * v[2 * j]
            k[j] -= 1. * .5 * v[2 * j + 1]
        if i == n - 1:
            print '0 0 0 setrgbcolor 1 setlinewidth'
        elif i == 0:
            print '1 0 0 setrgbcolor'
        elif i == 1:
            print '0 0.5 0 setrgbcolor'
        elif i == 2:
            print '0.3 0.3 0.3 setrgbcolor'
        plot_path(path, th, k)
        plot_ks(path, th, k)
    print '% th:', th
    print '% k:', k

path = [(100, 100, 'o'), (200, 250, 'o'), (350, 225, 'o'),
         (450, 350, 'c'), (450, 200, 'o'), (300, 100, 'o')]
if 0:
 path = [(100, 480, '['), (100, 300, ']'), (300, 100, 'o'),
        (500, 300, '['), (500, 480, ']'), (480, 500, '['),
        (120, 500, ']')]


path = [(100, 480, ']'), (100, 120, '['),
        (120, 100, ']'), (480, 100, '['),
        (500, 120, ']'), (500, 480, '['),
        (480, 500, ']'), (120, 500, '[')]

path = [(100, 120, '['), (120, 100, ']'),
        (140, 100, 'o'), (160, 100, 'o'), (180, 100, 'o'), (200, 100, 'o'),
        (250, 250, 'o'),
        (100, 200, 'o'), (100, 180, 'o'), (100, 160, 'o'), (100, 140, 'o')]

path = [(100, 350, 'o'), (225, 350, 'o'), (350, 450, 'o'),
        (450, 400, 'o'), (315, 205, 'o'), (300, 200, 'o'),
        (285, 205, 'o')]

if 0:
    path = []
    path.append((350, 600, 'c'))
    path.append((50, 450, 'c'))
    for i in range(10):
        path.append((50 + i * 30, 300 - i * 5, 'c'))
    for i in range(11):
        path.append((350 + i * 30, 250 + i * 5, 'c'))
    path.append((650, 450, 'c'))

solve(path)
print 'showpage'