text: """
  Cubic spline planner
  
  Author: Atsushi Sakai(@Atsushi_twi)
  
  """
  import math
  import numpy as np
  import bisect
  
  
  class Spline:
      """
      Cubic Spline class
      """
  
      def __init__(self, x, y):
          self.b, self.c, self.d, self.w = [], [], [], []
  
          self.x = x
          self.y = y
  
          self.nx = len(x)  # dimension of x
          h = np.diff(x)
  
          # calc coefficient c
          self.a = [iy for iy in y]
  
          # calc coefficient c
          A = self.__calc_A(h)
          B = self.__calc_B(h)
          self.c = np.linalg.solve(A, B)
          # print(self.c)
  
          # calc spline coefficient b and d
          for i in range(self.nx - 1):
              self.d.append((self.c[i + 1] - self.c[i]) / (3.0 * h[i]))
              tb = (self.a[i + 1] - self.a[i]) / h[i] - h[i] * \
                  (self.c[i + 1] + 2.0 * self.c[i]) / 3.0
              self.b.append(tb)
  
          self.b.append(0)
          self.d.append(0)
  
      def calc(self, t):
          """
          Calc position
  
          if t is outside of the input x, return None
  
          """
  
          if t < self.x[0]:
              return None
          elif t > self.x[-1]:
              return None
  
          i = self.__search_index(t)
          dx = t - self.x[i]
          result = self.a[i] + self.b[i] * dx + \
              self.c[i] * dx ** 2.0 + self.d[i] * dx ** 3.0
  
          return result
  
      def calcd(self, t):
          """
          Calc first derivative
  
          if t is outside of the input x, return None
          """
  
          if t < self.x[0]:
              return None
          elif t > self.x[-1]:
              return None
  
          i = self.__search_index(t)
          dx = t - self.x[i]
          result = self.b[i] + 2.0 * self.c[i] * dx + 3.0 * self.d[i] * dx ** 2.0
          return result
  
      def calcdd(self, t):
          """
          Calc second derivative
          """
  
          if t < self.x[0]:
              return None
          elif t > self.x[-1]:
              return None
  
          i = self.__search_index(t)
          dx = t - self.x[i]
          result = 2.0 * self.c[i] + 6.0 * self.d[i] * dx
          return result
  
      def __search_index(self, x):
          """
          search data segment index
          """
          return bisect.bisect(self.x, x) - 1
  
      def __calc_A(self, h):
          """
          calc matrix A for spline coefficient c
          """
          A = np.zeros((self.nx, self.nx))
          A[0, 0] = 1.0
          for i in range(self.nx - 1):
              if i != (self.nx - 2):
                  A[i + 1, i + 1] = 2.0 * (h[i] + h[i + 1])
              A[i + 1, i] = h[i]
              A[i, i + 1] = h[i]
  
          A[0, 1] = 0.0
          A[self.nx - 1, self.nx - 2] = 0.0
          A[self.nx - 1, self.nx - 1] = 1.0
          #  print(A)
          return A
  
      def __calc_B(self, h):
          """
          calc matrix B for spline coefficient c
          """
          B = np.zeros(self.nx)
          for i in range(self.nx - 2):
              B[i + 1] = 3.0 * (self.a[i + 2] - self.a[i + 1]) / \
                  h[i + 1] - 3.0 * (self.a[i + 1] - self.a[i]) / h[i]
          return B
  
  
  class NumericalAnalysisSpline(Spline):
      def __init__(self, x, y):
          self.nx = len(x)
          self.a, self.b = np.zeros(self.nx), np.zeros(self.nx)
          self.c, self.d = np.zeros(self.nx), np.zeros(self.nx)
          
          self.x = np.copy(x)
          self.a = np.copy(y)
          
          self.__cal()
  
  
      def __cal(self):
          l = np.zeros(self.nx)
          u = np.zeros(self.nx)
          z = np.zeros(self.nx)
          A = np.zeros(self.nx)
  
          h = np.diff(self.x)
          n = self.nx - 1
          for i in range(1, n):
              A[i] = 3 * (self.a[i + 1] - self.a[i]) / h[i] - 3 * (self.a[i] - self.a[i - 1]) / h[i - 1];
  
          l[0] = 1
      
          for i in range(1, n):
              l[i] = 2 * (self.x[i + 1] - self.x[i - 1]) - h[i - 1] * u[i - 1]
              u[i] = h[i] / l[i];
              z[i] = (A[i] - h[i - 1] * z[i - 1]) / l[i]
  
          l[n] = 1
          for j in range(n-1, -1, -1):
              self.c[j] = z[j] - u[j] * self.c[j + 1]
              self.b[j] = (self.a[j + 1] - self.a[j]) / h[j] - h[j] * (self.c[j + 1] + 2 * self.c[j]) / 3
              self.d[j] = (self.c[j + 1] - self.c[j]) / (3 * h[j])
  
  
  class Spline2D:
      """
      2D Cubic Spline class
  
      """
  
      def __init__(self, x, y):
          self.s = self.__calc_s(x, y)
          self.sx = NumericalAnalysisSpline(self.s, x)
          self.sy = NumericalAnalysisSpline(self.s, y)
  
      def __calc_s(self, x, y):
          dx = np.diff(x)
          dy = np.diff(y)
          self.ds = np.hypot(dx, dy)
          s = [0]
          s.extend(np.cumsum(self.ds))
          return s
  
      def calc_position(self, s):
          """
          calc position
          """
          x = self.sx.calc(s)
          y = self.sy.calc(s)
  
          return x, y
  
      def calc_curvature(self, s):
          """
          calc curvature
          """
          dx = self.sx.calcd(s)
          ddx = self.sx.calcdd(s)
          dy = self.sy.calcd(s)
          ddy = self.sy.calcdd(s)
          k = (ddy * dx - ddx * dy + 0.00001) / ((dx ** 2 + dy ** 2)**(3 / 2) + 0.00001)
          return k
  
      def calc_yaw(self, s):
          """
          calc yaw
          """
          dx = self.sx.calcd(s)
          dy = self.sy.calcd(s)
          yaw = math.atan2(dy, dx)
          return yaw