import numpy as np
from scipy.interpolate import CubicSpline

def recompute_tolerance(points, tol):
    points = np.array(points)
    
    total_length = np.sum(np.linalg.norm(points[1:] - points[:-1], axis=1))
    
    min_x, min_y = points.min(axis=0)
    max_x, max_y = points.max(axis=0)
    bounding_box_diagonal = np.linalg.norm([max_x - min_x, max_y - min_y])
    
    ratio = bounding_box_diagonal / total_length
    return tol * ratio

def simplify_polyline(points, tol):
    def douglas_peucker(points, tol_squared):
        if len(points) < 3:  
            return points

        start, end = points[0], points[-1]
        line_vector = end - start  
        line_length_squared = np.sum(line_vector ** 2) 

        start_to_points = points - start 

        start_to_points_3d = np.hstack([start_to_points, np.zeros((start_to_points.shape[0], 1))])
        line_vector_3d = np.hstack([line_vector, 0])

        if line_length_squared == 0:
            distances = np.sum(start_to_points ** 2, axis=1) 
        else:
            cross_products = np.cross(start_to_points_3d, line_vector_3d) 
            distances = (cross_products[:, 2] ** 2) / line_length_squared 

        max_distance_index = np.argmax(distances)

        if distances[max_distance_index] > tol_squared:
            left = douglas_peucker(points[:max_distance_index + 1], tol_squared)
            right = douglas_peucker(points[max_distance_index:], tol_squared)
            res = np.concatenate((left[:-1,:], right),axis=0)
            return res 
        else:
            res = np.array([start, end],dtype=np.float64)
            return res 
    
    points = np.array(points)
    tol_squared = tol ** 2 
    simplified_points = [points[0]] 
    
    for point in points[1:]:
        if np.sum((point - simplified_points[-1]) ** 2) >= tol_squared:
            simplified_points.append(point)
    
    simplified_points.append(points[-1]) 
    simplified_res = douglas_peucker(np.array(simplified_points), tol_squared)
    return simplified_res


def uniform_polyline(points, avg_length):
    points = np.array(points)
    uniform_points = []
    
    for i in range(len(points) - 1):
        start, end = points[i], points[i + 1]
        segment_length = np.linalg.norm(end - start)
        num_segments = max(int(segment_length / avg_length + 0.5), 1)
        
        cubic_segment = generate_cubicspline([start,end],isPeriodic=False)
        for t in np.linspace(0, 1, num_segments, endpoint=False):
            uniform_points.append(cubic_segment[int(len(cubic_segment)*t),:])

    if not np.array_equal(uniform_points[0], points[0]):
        uniform_points.insert(0, points[0])
    if not np.array_equal(uniform_points[-1], points[-1]):
        uniform_points.append(points[-1])

    return np.array(uniform_points)

def simplify_and_optimize_curve(points, initial_tol, avg_length):
    tol = recompute_tolerance(points, initial_tol)
    
    simplified_points = simplify_polyline(points, tol)
    print(len(simplified_points))
    
    uniform_points = uniform_polyline(simplified_points, avg_length)
    
    return uniform_points



def generate_cubicspline(points,isPeriodic=False):
    if isinstance(points,list):
        points = np.array(points)
    elif not isinstance(points,np.ndarray):
        print("Error: The type of points need to be list or ndarray!")
        return
    if isinstance(points, np.ndarray):
        if points.shape[0] == 2 and points.shape[1] != 2:
            points = points.T
    total_length = 0 
    for i in range(1, points.shape[0]):
        total_length += np.linalg.norm(points[i,:] - points[i-1,:])
    total_length = int(total_length)
    total_num = max(5,int(total_length + np.linalg.norm(points[-1,:] - points[0,:])))

    x = points[:,0]
    y = points[:,1]

    if isPeriodic:
        extended_x = np.concatenate((x, [x[0]]))
        extended_y = np.concatenate((y, [y[0]]))
        extended_t = np.linspace(0, 1, len(points) + 1)

        cs_x = CubicSpline(extended_t, extended_x, bc_type='periodic')
        cs_y = CubicSpline(extended_t, extended_y, bc_type='periodic')

        t_interp = np.linspace(0, 1, total_num)
        x_interp = cs_x(t_interp)
        y_interp = cs_y(t_interp)

        x_interp = np.concatenate((x_interp, [x_interp[0]]))
        y_interp = np.concatenate((y_interp, [y_interp[0]]))
        return np.stack([x_interp,y_interp],axis=1)
    else:
        t = np.linspace(0, 1, len(points))

        cs_x = CubicSpline(t, x, bc_type='natural')
        cs_y = CubicSpline(t, y, bc_type='natural')

        t_interp = np.linspace(0, 1, total_num)
        x_interp = cs_x(t_interp)
        y_interp = cs_y(t_interp)
        return np.stack([x_interp,y_interp],axis=1)

def find_nearest_cubicsegment(points, p):
    if isinstance(points,list):
        points = np.array(points)
    elif not isinstance(points,np.ndarray):
        print("Error: The type of points need to be list or ndarray!")
        return
    if isinstance(points, np.ndarray):
        if points.shape[0] == 2 and points.shape[1] != 2:
            points = points.T

    total_length = 0 
    for i in range(1, points.shape[0]):
        total_length += np.linalg.norm(points[i,:] - points[i-1,:])
    total_length = int(total_length)
    total_num = max(5,int(total_length + np.linalg.norm(points[-1,:] - points[0,:])))
    print(f"find_nearest_cubicsegment  total_num {total_num}")

    x = points[:, 0]
    y = points[:, 1]

    t = np.linspace(0, 1, len(points)) 
    cs_x = CubicSpline(t, x, bc_type='natural')
    cs_y = CubicSpline(t, y, bc_type='natural')

    t_interp = np.linspace(0, 1, total_num)
    x_interp = cs_x(t_interp)
    y_interp = cs_y(t_interp)

    distances = np.sqrt((x_interp - p[0])**2 + (y_interp - p[1])**2)

    nearest_index = np.argmin(distances)
    if nearest_index == 0:
        return 0
    elif nearest_index == len(t):
        return len(t)

    nearest_t = t_interp[nearest_index]

    for i in range(1, len(t)):
        if t[i-1] <= nearest_t < t[i]:
            return i
    return len(t)

def generate_spline_from_rdp(points, initial_tol=4.0, is_periodic=False, resolution_scale=1.0):
    points = np.array(points)

    tol = recompute_tolerance(points, initial_tol)

    simplified_points = simplify_polyline(points, tol)

    curve = generate_cubicspline(simplified_points, isPeriodic=is_periodic)
    if resolution_scale != 1.0:
        total_num = int(len(curve) * resolution_scale)
        t = np.linspace(0, 1, len(simplified_points))
        x = simplified_points[:, 0]
        y = simplified_points[:, 1]
        from scipy.interpolate import CubicSpline
        cs_x = CubicSpline(t, x, bc_type='periodic' if is_periodic else 'natural')
        cs_y = CubicSpline(t, y, bc_type='periodic' if is_periodic else 'natural')
        t_interp = np.linspace(0, 1, total_num)
        curve = np.stack([cs_x(t_interp), cs_y(t_interp)], axis=1)

    return curve, simplified_points
