###########################################################################################################################
###########################################################################################################################
###################### Uniform Cubic B-Spline Python Implementation, Object-Oriented Version With Const Speed Utility

class UCBSpline:
    # pre-calculate coefficients
    def __init__(self, p):
        self.p = p          # tuple or list of control points (x, y)
        n = len(p) - 1      # control points p0 ~ pn, i.e., p[0] ~ p[n]
        m = 3 + n + 1       # knots vector u0 ~um, i.e., u[0] ~ u[m]
        self.m = m
        step = 1 / (m - 2 * 3)
        self.u = 3 * [0] + [_u * step for _u in range(m - 2 * 3 + 1)] + [1] * 3     # knots vector
        num = n + 1 - 3     # number of curve segments
        self.num = num
        self.a1 = []
        self.a2 = []
        self.a3 = []
        self.a4 = []
        self.b1 = []
        self.b2 = []
        self.b3 = []
        self.b4 = []
        for i in range(num):
            self.a1.append((-p[i][0] + 3 * p[i + 1][0] - 3 * p[i + 2][0] + p[i + 3][0]) / 6.0)
            self.a2.append((3 * p[i][0] - 6 * p[i + 1][0] + 3 * p[i + 2][0]) / 6.0)
            self.a3.append((-p[i][0] + p[i + 2][0]) / 2.0)
            self.a4.append((p[i][0] + 4 * p[i + 1][0] + p[i + 2][0]) / 6.0)
            self.b1.append((-p[i][1] + 3 * p[i + 1][1] - 3 * p[i + 2][1] + p[i + 3][1]) / 6.0)
            self.b2.append((3 * p[i][1] - 6 * p[i + 1][1] + 3 * p[i + 2][1]) / 6.0)
            self.b3.append((-p[i][1] + p[i + 2][1]) / 2.0)
            self.b4.append((p[i][1] + 4 * p[i + 1][1] + p[i + 2][1]) / 6.0)
        self.SIMPSON = 100  # number of sub-intervals of composite simpson integration, must be even, usually 100 is good enough
        self.NEWTON = 0.0001  # epsilon, effective digits of the root calculated by Newton's method
    # calculate the value at t
    def __call__(self, t):
        if t < 0 or t >= 1:
            return None
        else:       # find the curve segment
            for i in range(3, self.m - 3):
                if self.u[i] <= t and t < self.u[i + 1]:
                    break
        idx = i - 3 # get coefficients
        a1 = self.a1[idx]
        a2 = self.a2[idx]
        a3 = self.a3[idx]
        a4 = self.a4[idx]
        b1 = self.b1[idx]
        b2 = self.b2[idx]
        b3 = self.b3[idx]
        b4 = self.b4[idx]
        t = (t - self.u[i]) / (self.u[i + 1] - self.u[i])   # map to the current curve segment
        return (a1 * t * t * t + a2 * t * t + a3 * t + a4, 
                b1 * t * t * t + b2 * t * t + b3 * t + b4, 255)    # 255 is the point's alpha value, to meet the TCAX's point structure requirement ((x, y, a), (x, y, a), ...)
    # total arc-length
    def length(self):
        from math import sqrt
        sqr = lambda x: x * x
        l = 0
        for i in range(self.num):
            a1 = self.a1[i]
            a2 = self.a2[i]
            a3 = self.a3[i]
            a4 = self.a4[i]
            b1 = self.b1[i]
            b2 = self.b2[i]
            b3 = self.b3[i]
            b4 = self.b4[i]
            f_grand = lambda t: sqrt(sqr(3 * a1 * t * t + 2 * a2 * t + a3) + sqr(3 * b1 * t * t + 2 * b2 * t + b3))       # integrand of UCB
            l += CompositeSimpson(f_grand, 0, 1, self.SIMPSON)
        return l
    # find t2 according to a specific arc-length s and a starting point t1
    def t2OfLength(self, t1, s):
        if t1 < 0 or t1 >= 1:
            return None
        else:       # find the curve segment
            for i in range(3, self.m - 3):
                if self.u[i] <= t1 and t1 < self.u[i + 1]:
                    break
        idx = i - 3 # get coefficients
        a1 = self.a1[idx]
        a2 = self.a2[idx]
        a3 = self.a3[idx]
        a4 = self.a4[idx]
        b1 = self.b1[idx]
        b2 = self.b2[idx]
        b3 = self.b3[idx]
        b4 = self.b4[idx]
        t1 = (t1 - self.u[i]) / (self.u[i + 1] - self.u[i])   # map to the current curve segment
        # Assumption: t1 and t are in the same piece of curve
        from math import sqrt
        sqr = lambda x: x * x
        f_grand = lambda t: sqrt(sqr(3 * a1 * t * t + 2 * a2 * t + a3) + sqr(3 * b1 * t * t + 2 * b2 * t + b3))       # integrand of UCB
        s1 = CompositeSimpson(f_grand, t1, 1, self.SIMPSON)     # length from the starting point t1 to the end of the current curve segment
        if s1 >= s:    # t1 and t are in the same segment of curve
            f_grand_d = lambda t: ((3 * a1 * t * t + 2 * a2 * t + a3) * (6 * a1 * t + 2 * a2) + (3 * b1 * t * t + 2 * b2 * t + b3) * (6 * b1 * t + 2 * b2 * t)) / f_grand(t)    # derivative of f_grand
            # using Newton's method to find the root of function f
            f = lambda t: CompositeSimpson(f_grand, t1, t, self.SIMPSON) - s
            fd = lambda t: CompositeSimpsonDerivative(f_grand, f_grand_d, t1, t, self.SIMPSON)   # derivative of f
            return NewtonMethod(f, fd, t1, self.NEWTON) * (self.u[i + 1] - self.u[i]) + self.u[i]   # find the t and map to the whole curve
        # otherwise, t is in next segment(s)
        else:
            if idx + 1 == self.num:     # no more curve segment
                return 1
            for i in range(idx + 1, self.num):
                a1 = self.a1[i]
                a2 = self.a2[i]
                a3 = self.a3[i]
                a4 = self.a4[i]
                b1 = self.b1[i]
                b2 = self.b2[i]
                b3 = self.b3[i]
                b4 = self.b4[i]
                f_grand = lambda t: sqrt(sqr(3 * a1 * t * t + 2 * a2 * t + a3) + sqr(3 * b1 * t * t + 2 * b2 * t + b3))       # integrand of UCB
                s0 = s1
                s1 += CompositeSimpson(f_grand, 0, 1, self.SIMPSON)
                if s1 >= s:     # we find at which segment the t lies
                    break
            if i == self.num:   # t is beyond the whole curve
                return 1
            f_grand_d = lambda t: ((3 * a1 * t * t + 2 * a2 * t + a3) * (6 * a1 * t + 2 * a2) + (3 * b1 * t * t + 2 * b2 * t + b3) * (6 * b1 * t + 2 * b2 * t)) / f_grand(t)    # derivative of f_grand
            # using Newton's method to find the root of function f
            f = lambda t: CompositeSimpson(f_grand, 0, t, self.SIMPSON) - (s - s0)
            fd = lambda t: CompositeSimpsonDerivative(f_grand, f_grand_d, 0, t, self.SIMPSON)   # derivative of f
            return NewtonMethod(f, fd, 0, self.NEWTON) * (self.u[i + 3 + 1] - self.u[i + 3]) + self.u[i + 3]   # find the t and map to the whole curve, note, here i is actually idx, should + 3 to be real i

# f=function, a=initial value, b=end value, n=number of intervals of size h, n must be even
def CompositeSimpson(f, a, b, n):
    h = (b - a) / n
    S = f(a)
    for i in range(1, n, 2):
        x = a + h * i
        S += 4 * f(x)
    for i in range(2, n - 1, 2):
        x = a + h * i
        S += 2 * f(x)
    S += f(b)
    return h * S / 3

# f=function, fd=derivative of f, t1=initial value (constant), t=end value (variable), n=number of intervals of size h, n must be even
def CompositeSimpsonDerivative(f, fd, t1, t, n):
    h = (t - t1) / n
    hd = 1 / n
    S = f(t1)
    for i in range(1, n, 2):
        x = t1 + h * i
        S += 4 * f(x)
    for i in range(2, n - 1, 2):
        x = t1 + h * i
        S += 2 * f(x)
    S += f(t)
    part1 = hd * S / 3
    # part2
    S = 0
    for i in range(1, n, 2):
        x = t1 + h * i
        xd = hd * i
        S += 4 * fd(x) * xd
    for i in range(2, n - 1, 2):
        x = t1 + h * i
        xd = hd * i
        S += 2 * fd(x) * xd
    S += fd(t)
    part2 = h * S / 3
    return part1 + part2

# f is the function whose root needs to be found, fd is the derivative of function f, x0 is the starting point, e is an epsilon, i.e., effective digits
def NewtonMethod(f, fd, x0, e):
    x = x0
    while True:
        x_new = x - f(x) / fd(x)
        if abs(x_new - x) < e:
            return x_new
        x = x_new


###########################################################################################################################
###########################################################################################################################
###################### Uniform Cubic B-Spline Python Implementation

def UniformCubicBSpline(p, t):
    n = len(p) - 1     # control points p0 ~ pn, i.e., p[0] ~ p[n]
    m = 3 + n + 1      # knots vector u0 ~um, i.e., u[0] ~ u[m]
    step = 1 / (m - 2 * 3)
    u = 3 * [0] + [_u * step for _u in range(m - 2 * 3 + 1)] + [1] * 3
    if t < 0 or t >= 1:
        return None
    else:
        for i in range(3, m - 3):
            if u[i] <= t and t < u[i + 1]:
                break
    return UniformCubicBSpline_Calc(p[i - 3], p[i - 2], p[i - 1], p[i], (t - u[i]) / (u[i + 1] - u[i]))    # can use either calc or calc2

def UniformCubicBSpline_Calc(p1, p2, p3, p4, t):
    a1 = (-p1[0] + 3 * p2[0] - 3 * p3[0] + p4[0]) / 6.0
    a2 = (3 * p1[0] - 6 * p2[0] + 3 * p3[0]) / 6.0
    a3 = (-p1[0] + p3[0]) / 2.0
    a4 = (p1[0] + 4 * p2[0] + p3[0]) / 6.0
    b1 = (-p1[1] + 3 * p2[1] - 3 * p3[1] + p4[1]) / 6.0
    b2 = (3 * p1[1] - 6 * p2[1] + 3 * p3[1]) / 6.0
    b3 = (-p1[1] + p3[1]) / 2.0
    b4 = (p1[1] + 4 * p2[1] + p3[1]) / 6.0
    return (a1 * t * t * t + a2 * t * t + a3 * t + a4, 
            b1 * t * t * t + b2 * t * t + b3 * t + b4, 255)    # 255 is the point's alpha value, to meet the TCAX's point structure requirement ((x, y, a), (x, y, a), ...)

def UniformCubicBSpline_Calc2(p1, p2, p3, p4, t):
    A1 = (1 - t) * (1 - t) * (1 - t) / 6.0
    A2 = (3 * t * t * t - 6 * t * t + 4) / 6.0
    A3 = (-3 * t * t * t + 3 * t * t + 3 * t + 1) / 6.0
    A4 = t * t * t / 6.0
    return (A1 * p1[0] + A2 * p2[0] + A3 * p3[0] + A4 * p4[0], 
            A1 * p1[1] + A2 * p2[1] + A3 * p3[1] + A4 * p4[1], 255)    # 255 is the point's alpha value, to meet the TCAX's point structure requirement ((x, y, a), (x, y, a), ...)