# Utilities for piecewise cornu representation of curves

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

from math import atan2
from math import hypot
from math import sqrt
from math import sin
from math import cos
from math import fabs
from math import floor
from math import pi

import clothoid
import cornu


class Segment(object):
    def __init__(self, z0, z1, th0, th1):
        self.z0 = z0
        self.z1 = z1
        self.th0 = th0
        self.th1 = th1
        self.compute()

    def __repr__(self):
        return '[' + repr(self.z0) + repr(self.z1) + ' ' + repr(self.th0) + ' ' + repr(self.th1) + ']'

    def compute(self):
        dx = self.z1[0] - self.z0[0]
        dy = self.z1[1] - self.z0[1]
        chord = hypot(dy, dx)
        chth = atan2(dy, dx)
        k0, k1 = clothoid.solve_clothoid(self.th0, self.th1)
        charc = clothoid.compute_chord(k0, k1)

        self.chord = chord
        self.chth = chth
        self.k0, self.k1 = k0, k1
        self.charc = charc
        self.arclen = chord / charc
        self.thmid = self.chth - self.th0 + 0.5 * self.k0 - 0.125 * self.k1

        self.setup_xy_fresnel()

    def setup_xy_fresnel(self):
        k0, k1 = self.k0, self.k1
        if k1 == 0:
            k1 = 1e-6  # hack
        if k1 != 0:
            sqrk1 = sqrt(2 * abs(k1))
            t0 = (k0 - .5 * k1) / sqrk1
            t1 = (k0 + .5 * k1) / sqrk1
            (y0, x0) = cornu.eval_cornu(t0)
            (y1, x1) = cornu.eval_cornu(t1)
            chord_th = atan2(y1 - y0, x1 - x0)
            chord = hypot(y1 - y0, x1 - x0)
            scale = self.chord / chord
            if k1 >= 0:
                th = self.chth - chord_th
                self.mxx = scale * cos(th)
                self.myx = scale * sin(th)
                self.mxy = -self.myx
                self.myy = self.mxx
            else:
                th = self.chth + chord_th
                self.mxx = scale * cos(th)
                self.myx = scale * sin(th)
                self.mxy = self.myx
                self.myy = -self.mxx
                # rotate -chord_th, flip top/bottom, rotate self.chth
            self.x0 = self.z0[0] - (self.mxx * x0 + self.mxy * y0)
            self.y0 = self.z0[1] - (self.myx * x0 + self.myy * y0)

    def th(self, s):
        u = s / self.arclen - 0.5
        return self.thmid + (0.5 * self.k1 * u + self.k0) * u

    def xy(self, s):
        # using fresnel integrals; polynomial approx might be better
        u = s / self.arclen - 0.5
        k0, k1 = self.k0, self.k1
        if k1 == 0:
            k1 = 1e-6  # hack
        if k1 != 0:
            sqrk1 = sqrt(2 * abs(k1))
            t = (k0 + u * k1) / sqrk1
            (y, x) = cornu.eval_cornu(t)
            return [self.x0 + self.mxx * x + self.mxy * y,
                    self.y0 + self.myx * x + self.myy * y]

    def find_extrema(self):
        # find solutions of th(s) = 0 mod pi/2
        # todo: find extra solutions when there's an inflection
        th0 = self.thmid + 0.125 * self.k1 - 0.5 * self.k0
        th1 = self.thmid + 0.125 * self.k1 + 0.5 * self.k0
        twooverpi = 2 / pi
        n0 = int(floor(th0 * twooverpi))
        n1 = int(floor(th1 * twooverpi))
        if th1 > th0:
            signum = 1
        else:
            signum = -1
        result = []
        for i in range(n0, n1, signum):
            th = pi / 2 * (i + 0.5 * (signum + 1))
            a = .5 * self.k1
            b = self.k0
            c = self.thmid - th
            if a == 0:
                u1 = -c / b
                u2 = 1000
            else:
                sqrtdiscrim = sqrt(b * b - 4 * a * c)
                u1 = (-b - sqrtdiscrim) / (2 * a)
                u2 = (-b + sqrtdiscrim) / (2 * a)
            if -0.5 <= u1 < 0.5:
                result.append(self.arclen * (u1 + 0.5))
            if -0.5 <= u2 < 0.5:
                result.append(self.arclen * (u2 + 0.5))
        return result


class Curve(object):
    def __init__(self, segs):
        self.segs = segs
        self.compute()

    def compute(self):
        arclen = 0
        sstarts = []
        for seg in self.segs:
            sstarts.append(arclen)
            arclen += seg.arclen

        self.arclen = arclen
        self.sstarts = sstarts

    def th(self, s, deltas=False):
        u = s / self.arclen
        s = self.arclen * (u - floor(u))
        if s == 0 and not deltas:
            s = self.arclen
        i = 0
        while i < len(self.segs) - 1:
            # binary search would make a lot of sense here
            snext = self.sstarts[i + 1]
            if s < snext or (not deltas and s == snext):
                break
            i += 1
        return self.segs[i].th(s - self.sstarts[i])

    def xy(self, s):
        u = s / self.arclen
        s = self.arclen * (u - floor(u))
        i = 0
        while i < len(self.segs) - 1:
            # binary search would make a lot of sense here
            if s <= self.sstarts[i + 1]:
                break
            i += 1
        return self.segs[i].xy(s - self.sstarts[i])

    def find_extrema(self):
        result = []
        for i in range(len(self.segs)):
            seg = self.segs[i]
            for s in seg.find_extrema():
                result.append(s + self.sstarts[i])
        return result

    def find_breaks(self):
        result = []
        for i in range(len(self.segs)):
            pseg = self.segs[(i + len(self.segs) - 1) % len(self.segs)]
            seg = self.segs[i]
            th = clothoid.mod_2pi(pseg.chth + pseg.th1 - (seg.chth - seg.th0))
            print('% pseg', pseg.chth + pseg.th1, 'seg', seg.chth - seg.th0)
            pisline = pseg.k0 == 0 and pseg.k1 == 0
            sisline = seg.k0 == 0 and seg.k1 == 0
            if fabs(th) > 1e-3 or (pisline and not sisline) or (sisline and not pisline):
                result.append(self.sstarts[i])
        return result
