import numpy as np
from sklearn.cluster import DBSCAN
from scipy.spatial import ConvexHull
from scipy.optimize import least_squares

CLASSES = [
    'Betula pendula',
    'Fagus sylvatica', 
    'Picea abies',
    'Pinus sylvestris'
]

def fit_circle(x, y):
    """Fit a circle to given x, y points."""
    def calc_radius(params):
        cx, cy, r = params
        return np.sqrt((x - cx)**2 + (y - cy)**2) - r

    # Initial guess for circle parameters: center (mean x, mean y), radius
    x_m, y_m = np.mean(x), np.mean(y)
    r_initial = np.mean(np.sqrt((x - x_m)**2 + (y - y_m)**2))
    initial_params = [x_m, y_m, r_initial]

    # Use least squares optimization to fit the circle
    result = least_squares(calc_radius, initial_params)
    cx, cy, r = result.x
    return cx, cy, r
    
def remove_noise(points, eps=0.05, min_samples=10):
    """
    Remove noise from points using DBSCAN clustering.

    Args:
        points (numpy.ndarray): Array of shape (N, 3) with columns [x, y, z].
        eps (float): Maximum distance between two samples to consider them as in the same neighborhood.
        min_samples (int): Minimum number of points to form a dense region.
    
    Returns:
        numpy.ndarray: Denoised points.
    """
    clustering = DBSCAN(eps=eps, min_samples=min_samples).fit(points)
    labels = clustering.labels_
    try:
        largest_cluster = labels == np.argmax(np.bincount(labels[labels >= 0]))
    except Exception:
        raise RuntimeError("Error in DBH calculation")
    return points[largest_cluster]

def calculate_dbh(points, dbh_height=1.3, height_buffer=0.1, eps=0.05, min_samples=10):
    """
    Calculate the Diameter at Breast Height (DBH) of a tree from point cloud data.

    Args:
        points (numpy.ndarray): Array of shape (N, 3) with columns [x, y, z].
        dbh_height (float): Height at which DBH is measured (default is 1.3 meters).
        height_buffer (float): Range around dbh_height to include points (default is ±0.1 meters).
    
    Returns:
        float: DBH in meters.
    """
    z_min, z_max = dbh_height - height_buffer, dbh_height + height_buffer
    trunk_points = points[(points[:, 2] >= (z_min)) & (points[:, 2] <= (z_max))]
    
    if trunk_points.shape[0] < 3:
        raise ValueError("Not enough points to calculate DBH.")
    
    # Remove noise
    denoised_points = remove_noise(trunk_points[:, :2], eps=eps, min_samples=min_samples)
    denoised_points = np.hstack((denoised_points, np.full((denoised_points.shape[0], 1), dbh_height)))

    if denoised_points.shape[0] < 3:
        raise ValueError("Not enough points left after noise removal.")
    
    # Fit a circle to the trunk points
    x, y = denoised_points[:, 0], denoised_points[:, 1]
    cx, cy, radius = fit_circle(x, y)
    
    # Generate points along the fitted circle for visualization
    theta = np.linspace(0, 2 * np.pi, 100)
    circle_x = cx + radius * np.cos(theta)
    circle_y = cy + radius * np.sin(theta)
    circle_points = np.column_stack((circle_x, circle_y, np.full_like(circle_x, dbh_height)))
    
    # Calculate DBH (Diameter = 2 * radius)
    dbh = 2 * radius
    return dbh, circle_points

def calc_canopy_volume(points, threshold, height, z_min):
    '''
    Calculates the canopy points for a given point cloud data of a tree
    and calculates the volume using the Qhull algorithm
    
    Args:
        points: point cloud data
        threshold: z_threshold in percentage
        height, z_min
        
    Returns:
        canopy_volume, canopy_points
    '''
    z_threshold = z_min + (threshold / 100) * height
    canopy_points = points[points[:, 2] >= z_threshold]
    clustering = DBSCAN(eps=1.0, min_samples=10).fit(canopy_points[:, :3])
    labels = clustering.labels_
    canopy_points = canopy_points[labels != -1]
    
    if canopy_points.shape[0] < 4:
        canopy_volume = None
    else:
        '''
        Uses the QuickHull algorithm which uses a divide-and-conquer approach.
        It selects the 2 leftmost and rightmost points on a 2D plane; 
        These are part of the ConvexHull.
        Then it selects the point farthest away from the line joining the 
        above 2 points and adds it to the ConvexHull.
        The points enclosed within that shape cannot be part of the 
        ConvexHull and are ignored. This process is then repeated until 
        all points are either part of the ConvexHull or contained inside it.
        '''
        hull = ConvexHull(canopy_points)
        canopy_volume = hull.volume
    
    return canopy_volume, canopy_points