import numpy as np
import overlap

from polygon_triangulate import polygon_triangulate, PlotTriangulation
from tetrahedralize_triangles import tetrahedralize

def measure_volume(V_tet, T, origin, r):

    sphere = overlap.Sphere(origin, r)
    volume = 0.0

    for tet in T:
        vertices = V_tet[tet]
        overlap_tet = overlap.Tetrahedron(vertices)
        overlap_volume = overlap.overlap(sphere, overlap_tet)
        volume += overlap_volume

    return volume

def same_point(x, y):
    return np.linalg.norm(x - y) < 1e-10

def remove_duplicate_contour_points(P):
    n = len(P)
    for i in range(n):
        j = (i + 1) % n
        if same_point(P[i], P[j]):
            return remove_duplicate_contour_points(np.delete(P, i, axis=0))
    return P

def measure_cumulative_volume(z, P, thickness, origin, R):
    '''
    Integration of the intersecting volume of a thick polygon and a sphere of increasing radius.

    Parameters
        ----------
        z : float
            The z depth-wise coordinate of the thick polygon.
        P : (N, 2) array_like
            A real array of ordered contour points of the polygon.
        thickness : float
            The thickness of the polygon.
        origin : (3,) array_like
            The center of the sphere from which to integrate from.
        R : (M,) array_like
            The discrete sphere radii from which to measure the polygon in.
        
        Returns
        -------
        mu: (M,) array_like
            The intersecting volumes of the sphere and the thick polygon at radii determined by R.
    '''

    P = np.array(P)
    origin = np.array(origin)

    P = remove_duplicate_contour_points(P)

    z0 = z - thickness / 2
    z1 = z + thickness / 2
    _, V_tri, F = polygon_triangulate(P)

    V_tet, T = tetrahedralize(V_tri, F, z0, z1)

    mu = np.zeros(len(R))
    for i, r in enumerate(R):

        if r <= 0:
            continue

        mu[i] = measure_volume(V_tet, T, origin, r)

    return mu

def measure_cumulative_points(z, X, thickness, origin, R):
    '''
    Integration of the points placed in a thick section integrated intersecting with a sphere of increasing size.

    Parameters
        ----------
        z : float
            The z depth-wise coordinate of the section with the points.
        X : (N, 2) array_like
            A real array of points inside the section.
        thickness : float
            The thickness of the section of which the points are in.
        origin : (3,) array_like
            The center of the sphere from which to integrate from.
        R : (M,) array_like
            The discrete sphere radii from which to measure the points inside.
        
        Returns
        -------
        mu: (M,) array_like
            The integrated point masses inside the sphere of radii determined by R.
    '''

    oz = origin[2]
    oxy = origin[:2]

    dxy = np.array([np.linalg.norm(x - oxy) for x in X])

    mu = np.zeros(len(R))

    if z == oz:
        for i, r in enumerate(R):
            s = np.sqrt(np.clip(r**2 - dxy**2, 0, None))
            mu[i] = np.sum(np.clip(2*s/thickness, 0, 1))

        return mu

    # plane is below, mirror it above to simplify calculations
    if z < oz:
        z = z + 2*(oz - z)

    z0 = z - thickness / 2
    z1 = z + thickness / 2

    d0 = z0 - oz

    for i, r in enumerate(R):
        h = np.sqrt(np.clip(r**2 - dxy**2, 0, None))
        s = h - d0
        mu[i] = np.sum(np.clip(s/thickness, 0, 1))

    return mu