import math

def add(a=1,A=(0,0),b=0,B=(0,0)):
    '''allows Vector addition, a,b are scalars, allows subtraction for b=-1, returns a vector'''
    aX,aY = A
    bX,bY = B
    return a*aX+b*bX,a*aY+b*bY

def length(A=(0,0)):
    '''find the length of a vector'''
    ax,ay = A
    length = math.hypot(ax,ay)
    return length

def rotate(angle,vector):
    ''' accepts an angle in degrees and a vector, returns the vector rotated by that angle'''
    vx,vy = vector
    theta = math.atan2(vy,vx)
    length = math.hypot(vx,vy)
    new_angle = theta+angle
    wx = length*math.cos(new_angle)
    wy = length*math.sin(new_angle)
    return wx,wy
    
def normalise(A, new_length = 1):
    '''normalises the length of a vector'''
    ax,ay = A
    length = math.hypot(ax,ay)
    if    length == 0:
        return 0,0
    else: 
        bx = ax/length
        by = ay/length
    return bx*new_length,by*new_length

def dot(A,B):
    '''return the dot product of two vectors'''
    ax,ay = A
    bx,by = B
    dotprod = ax*bx+ay*by
    return dotprod

def direction_to(A,B):
    ax,ay = A
    bx,by = B
    C = ax-bx, ay-by
    cx,cy = normalise(C)
    return cx,cy
    
def angle_to(A,B = (0,0)):
    """ returns [0,2 pi] with branch cut along positive x axis instead of [-pi, pi] with branch cut along negative x axis"""
#    ax,ay = A
#    bx,by = B
#    return (math.atan2(ay-by, ax-bx)+2*math.pi)%(2*math.pi)
    return (math.atan2(A[1]-B[1], A[0]-B[0])+2*math.pi)%(2*math.pi)

def NRT_angle_to(A,B = (0,0)):
    """ this function used at startup - not run time, made for profiling purposes. """
#    ax,ay = A
#    bx,by = B
#    return (math.atan2(ay-by, ax-bx)+2*math.pi)%(2*math.pi)
    return (math.atan2(A[1]-B[1], A[0]-B[0])+2*math.pi)%(2*math.pi)

def distance_to(A,B):
    ax,ay = A
    bx,by = B
    distance = math.hypot(ax-bx,ay-by)
    return distance

def sign(A):
    if A>0:
        return 1
    if A == 0:
        return 0
    if A<0:
        return -1