import numpy as np
import math

# This function fits a circle to the 3 coordinates provided as inputs and then calculates the radius of that circle 
def rad_from_points(x1, y1, x2, y2, x3, y3):
    '''
    takes coordinates of three points, (x1, y1), (x2, y2), and (x3, y3)
    and returns the radius (r), and the coordinates of the center (xc, yc)
    of the circle that passes through the three points.

    The center of the circle can be found by finding the intersection
    of the line that bisects the segment formed by (x1, y1) and (x2, y2),
    and the line that bisects the segment formed by (x2, y2) and (x3, y3).
    '''

    ma = (y2 - y1)/(x2 - x1)
    mb = (y3 - y2)/(x3 - x2)

    xc = (ma*mb*(y1 - y3) + mb*(x1 + x2) - ma*(x2 + x3))/(2*(mb - ma))
    yc = -1/ma*(xc - (x1 + x2)/2) + (y1 + y2)/2

    if ma == mb:
        r = float(np.inf)

    else:
        r = float(np.hypot(xc - x1, yc - y1))

    return(r, xc, yc)

# This function takes a list of x and y coordinates (as the inputs) and a scaling parameter (input) that sets the effective length scale to calculate curvature. 
# It outputs a list of signed curvature values
# For example: one can calculate curvature between 3 consecutive points (right next to eachother), or between three non-consecutive points (some set distance apart) 
def radius_of_curvature(x_path, y_path,scale):
    '''
    takes a path and returns the signed curvature value at each point (along w/ the centers
    that form the evolute of the curve)
    '''

    r = []
    xcs = []
    ycs = []
    DegreeOfCurv = []

    num_points = len(x_path)
    for i in range(int(scale),int(num_points-scale)):
        # points
        x1 = x_path[i-int(scale)]
        y1 = y_path[i-int(scale)]
        x2 = x_path[i]
        y2 = y_path[i]
        x3 = x_path[i+int(scale)]
        y3 = y_path[i+int(scale)]

        # fit circle
        rad, xc, yc = rad_from_points(x1, y1, x2, y2, x3, y3)

        # get vector normal to path for sign of curvature
        nv1 = np.cross(np.array([x2 - x1, y2 - y1, 0]), np.array([0 ,0, 1]))
        nv2 = np.cross(np.array([x3 - x2, y3 - y2, 0]), np.array([0 ,0, 1]))

        nv = np.average([nv1, nv2], axis = 0)

        # get sign of dot product (positive curvature is outward)
        # flip sign if you want positive curvature to be inward
        align = np.sign(np.dot(nv[0:2], np.array([x2 - xc, y2 - yc])))

        if rad == 0:
            r.append(np.nan)
        else:
            r.append(align * 1./rad)
            
        xcs.append(xc)
        ycs.append(yc)

    return(r, xcs, ycs)