# Convert piecewise cubic into piecewise clothoid representation.

from __future__ import absolute_import
from __future__ import division
from __future__ import print_function

from math import hypot
from math import atan2
import sys

import clothoid
import pcorn
import tocubic

import offset


def read_bz(f):
    result = []
    for l in f.readlines():
        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])
        x12, y12 = 0.5 * (bz[1][0] + bz[2][0]), 0.5 * (bz[1][1] + bz[2][1])
        x23, y23 = 0.5 * (bz[2][0] + bz[3][0]), 0.5 * (bz[2][1] + bz[3][1])
        xl2, yl2 = 0.5 * (x01 + x12), 0.5 * (y01 + y12)
        xr1, yr1 = 0.5 * (x12 + x23), 0.5 * (y12 + y23)
        xm, ym = 0.5 * (xl2 + xr1), 0.5 * (yl2 + yr1)
        bzl = [bz[0], (x01, y01), (xl2, yl2), (xm, ym)]
        bzr = [(xm, ym), (xr1, yr1), (x23, y23), bz[3]]
        segs = cubic_bz_to_pcorn(bzl, 0.5 * thresh)
        segs.extend(cubic_bz_to_pcorn(bzr, 0.5 * thresh))
        return segs


def bzs_to_pcorn(bzs, thresh=1e-9):
    result = []
    for sp in bzs:
        rsp = []
        for bz in sp:
            if len(bz) == 2:
                dx = bz[1][0] - bz[0][0]
                dy = bz[1][1] - bz[0][1]
                th = atan2(dy, dx)
                rsp.append(pcorn.Segment(bz[0], bz[1], 0, 0))
            else:
                rsp.extend(cubic_bz_to_pcorn(bz, thresh))
        result.append(rsp)
    return result


def plot_segs(segs):
    for i in range(len(segs)):
        seg = segs[i]
        if i == 0:
            print(seg.z0[0], seg.z0[1], 'moveto')
        print(seg.z1[0], seg.z1[1], 'lineto')
    print('stroke')
    for i in range(len(segs)):
        seg = segs[i]
        if i == 0:
            print('gsave', seg.z0[0], seg.z0[1], 'translate circle fill grestore')
        print('gsave', seg.z1[0], seg.z1[1], 'translate circle fill grestore')


def test_to_pcorn():
    C1 = 0.55228
    bz = [(100, 100), (100 + 400 * C1, 100), (500, 500 - 400 * C1), (500, 500)]
    for i in range(0, 13):
        thresh = .1 ** i
        segs = cubic_bz_to_pcorn(bz, thresh)
        plot_segs(segs)
        print(thresh, len(segs), file=sys.stderr)
        print('0 20 translate')


if __name__ == '__main__':
    with open(sys.argv[1]) as f:
        bzs = read_bz(f)
    rsps = bzs_to_pcorn(bzs, 1)
    # print(rsps)
    tocubic.plot_prolog()
    print('grestore')
    print('1 -1 scale 0 -720 translate')
    print('/ss 1.5 def')
    print('/circle { ss 0 moveto currentpoint exch ss sub exch ss 0 360 arc } bind def')
    tot = 0
    for segs in rsps:
        curve = pcorn.Curve(segs)
        # curve = offset.offset(curve, 10)
        print('%', curve.arclen)
        print('%', curve.sstarts)
        if 0:
            print('gsave 1 0 0 setrgbcolor')
            cmd = 'moveto'
            for i in range(100):
                s = i * .01 * curve.arclen
                x, y = curve.xy(s)
                th = curve.th(s)
                sth = 5 * sin(th)
                cth = 5 * cos(th)
                print(x, y, cmd)
                cmd = 'lineto'
            print('closepath stroke grestore')
        for i in range(100):
            s = i * .01 * curve.arclen
            x, y = curve.xy(s)
            th = curve.th(s)
            sth = 5 * sin(th)
            cth = 5 * cos(th)
            if 0:
                print(x - cth, y - sth, 'moveto')
                print(x + cth, y + sth, 'lineto stroke')
        if 1:
            for s in curve.find_breaks():
                print('gsave 0 1 0 setrgbcolor')
                x, y = curve.xy(s)
                print(x, y, 'translate 2 dup scale circle fill')
                print('grestore')
        # plot_segs(segs)

        print('gsave 0 0 0 setrgbcolor')
        optim = 3
        thresh = 1e-2
        new_bzs = tocubic.pcorn_curve_to_bzs(curve, optim, thresh)
        tot += len(new_bzs)
        plot_bzs([new_bzs], (0, 0), 1, True)
        print('grestore')
    print('grestore')
    print('/Helvetica 12 selectfont')
    print('36 720 moveto (thresh=%g optim=%d) show' % (thresh, optim))
    print('( tot segs=%d) show' % tot)
    print('showpage')

    # plot_bzs(bzs, (100, 100), 1)
