import math

def euclidean_distance(point1, point2):
  '''
  Args:
    point1: type array two values [x, y]
    point2: type array two values [x, y]
  Returns:
    Distance of two points
  Descriptions:
    Calculate Euclidian distance of two points in Euclidian space
  '''

  return math.sqrt((point1[0]-point2[0])**2 + (point1[1]-point2[1])**2)

def curve_length(curve):
  '''
  Args:
    points: type arrays two values [[x, y], [x, y]]
  Returns:
    acc_length: curve length
  Descriptions:
    Calculate the length of the curve
  '''

  acc_length = 0
  for i in range(0, len(curve)-1):
    acc_length += euclidean_distance(curve[i], curve[i+1])
  return acc_length

def extend_point_on_line(point1, point2, distance):
  '''
  Args:
    point1: type array two values [x, y]
    point2: type array two values [x, y]
    distance: type float
  Returns:
    A new point, point3, which is on the same
    line generated by point1 and point2
  '''

  norm = round(distance / euclidean_distance(point1, point2), 2)
  new_point_x = point2[0] + norm * (point1[0] - point2[0])
  new_point_y = point2[1] + norm * (point1[1] - point2[1])
  return [new_point_x, new_point_y]

def rotate_curve(curve, thetaRad):
  '''
  Args:
    curve: original curve, type array two values [[x, y], [x, y]]
    thetaRad: rotation angle type float
  Returns:
    rot_curve: rotated curve
  Descriptions:
    Rotate the curve around the origin
  '''

  rot_curve = []
  for i in range(len(curve)):
    x_cord = math.cos(-1 * thetaRad) * curve[i][0] - math.sin(-1 * thetaRad) * curve[i][1]
    y_cord = math.sin(-1 * thetaRad) * curve[i][0] + math.cos(-1 * thetaRad) * curve[i][1]
    rot_curve.append([x_cord, y_cord])
  return rot_curve
