###########################################################################################################################
###########################################################################################################################
###################### 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)
        num = len(p) - 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
        t *= self.num
        i = int(t)   # get coefficients
        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]
        t -= i
        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]
            b1 = self.b1[i]
            b2 = self.b2[i]
            b3 = self.b3[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
    # x derivative at t
    def xd(self, t):
        if t < 0 or t >= 1:
            return None
        t *= self.num
        i = int(t)   # get coefficients
        a1 = self.a1[i]
        a2 = self.a2[i]
        t -= i
        return 6 * a1 * t + 2 * a2
    # y derivative at t
    def yd(self, t):
        if t < 0 or t >= 1:
            return None
        t *= self.num
        i = int(t)   # get coefficients
        b1 = self.b1[i]
        b2 = self.b2[i]
        t -= i
        return 6 * b1 * t + 2 * b2
    # 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
        t1 *= self.num
        i = int(t1)   # get coefficients
        a1 = self.a1[i]
        a2 = self.a2[i]
        a3 = self.a3[i]
        b1 = self.b1[i]
        b2 = self.b2[i]
        b3 = self.b3[i]
        t1 -= i
        # 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) + i) / self.num   # find the t and map to the whole curve
        # otherwise, t is in next segment(s)
        else:
            i0 = i + 1
            if i0 == self.num:     # no more curve segment
                return 1
            for i in range(i0, self.num):
                a1 = self.a1[i]
                a2 = self.a2[i]
                a3 = self.a3[i]
                b1 = self.b1[i]
                b2 = self.b2[i]
                b3 = self.b3[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) + i) / self.num   # 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):
    if t < 0 or t >= 1:
        return None
    num = len(p) - 3    # number of curve segments
    t *= num
    i = int(t)
    return UniformCubicBSpline_Calc(p[i], p[i + 1], p[i + 2], p[i + 3], t - 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), ...)