# Convert piecewise cubic into piecewise clothoid representation.

from math import *

import clothoid
import pcorn
import tocubic

import offset

def read_bz(f):
    result = []
    for l in f.xreadlines():
        s = l.split()
        if len(s) > 0:
            cmd = s[-1]
            #print s[:-1], cmd
            if cmd == 'm':
                sp = []
                result.append(sp)
                curpt = [float(x) for x in s[0:2]]
                startpt = curpt
            elif cmd == 'l':
                newpt = [float(x) for x in s[0:2]]
                sp.append((curpt, newpt))
                curpt = newpt
            elif cmd == 'c':
                c1 = [float(x) for x in s[0:2]]
                c2 = [float(x) for x in s[2:4]]
                newpt = [float(x) for x in s[4:6]]
                sp.append((curpt, c1, c2, newpt))
                curpt = newpt
    return result

def plot_bzs(bzs, z0, scale, fancy = False):
    for sp in bzs:
        for i in range(len(sp)):
            bz = sp[i]
            tocubic.plot_bz(bz, z0, scale, i == 0)
        print 'stroke'
        if fancy:
            for i in range(len(sp)):
                bz = sp[i]

                x0, y0 = z0[0] + scale * bz[0][0], z0[1] + scale * bz[0][1]
                print 'gsave', x0, y0, 'translate circle fill grestore'
                if len(bz) == 4:
                    x1, y1 = z0[0] + scale * bz[1][0], z0[1] + scale * bz[1][1]
                    x2, y2 = z0[0] + scale * bz[2][0], z0[1] + scale * bz[2][1]
                    x3, y3 = z0[0] + scale * bz[3][0], z0[1] + scale * bz[3][1]
                    print 'gsave 0.5 setlinewidth', x0, y0, 'moveto'
                    print x1, y1, 'lineto stroke'
                    print x2, y2, 'moveto'
                    print x3, y3, 'lineto stroke grestore'
                    print 'gsave', x1, y1, 'translate 0.75 dup scale circle fill grestore'
                    print 'gsave', x2, y2, 'translate 0.75 dup scale circle fill grestore'
                    print 'gsave', x3, y3, 'translate 0.75 dup scale circle fill grestore'
            
        

def measure_bz_cloth(seg, bz, n = 100):
    bz_arclen = tocubic.bz_arclength_rk4(bz)
    arclen_ratio = seg.arclen / bz_arclen
    dbz = tocubic.bz_deriv(bz)
    
    def measure_derivs(x, ys):
        dx, dy = tocubic.bz_eval(dbz, x)
        ds = hypot(dx, dy)
        s = ys[0] * arclen_ratio
        dscore = ds * (tocubic.mod_2pi(atan2(dy, dx) - seg.th(s)) ** 2)
        #print s, atan2(dy, dx),  seg.th(s)
        return [ds, dscore]
    dt = 1./n
    t = 0
    ys = [0, 0]
    for i in range(n):
        dydx = measure_derivs(t, ys)
        tocubic.rk4(ys, dydx, t, dt, measure_derivs)
        t += dt
    return ys[1]

def cubic_bz_to_pcorn(bz, thresh):
    dx = bz[3][0] - bz[0][0]
    dy = bz[3][1] - bz[0][1]
    dx1 = bz[1][0] - bz[0][0]
    dy1 = bz[1][1] - bz[0][1]
    dx2 = bz[3][0] - bz[2][0]
    dy2 = bz[3][1] - bz[2][1]
    chth = atan2(dy, dx)
    th0 = tocubic.mod_2pi(chth - atan2(dy1, dx1))
    th1 = tocubic.mod_2pi(atan2(dy2, dx2) - chth)
    seg = pcorn.Segment(bz[0], bz[3], th0, th1)
    err = measure_bz_cloth(seg, bz)
    if err < thresh:
        return [seg]
    else:
        # de Casteljau
        x01, y01 = 0.5 * (bz[0][0] + bz[1][0]), 0.5 * (bz[0][1] + bz[1][1])