import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D

def calculate_face_center(face_points):
    """
    Compute the geometric center (centroid) of a possibly non-planar face.
    Args:
        face_points (list of list): List of points defining the face.
                                    Each point is a 3D list [x, y, z].
    Returns:
        np.array: The computed face center as a 3D vector.
    """
    points = np.array(face_points)
    p_avg = np.mean(points, axis=0)  # Step 1: Geometric average of vertices

    sumA = np.zeros(3)
    N = len(points)
    for i in range(N):
        p = points[i]
        p_next = points[(i + 1) % N]  # Wrap around for last vertex
        sumA += np.cross(p_next - p, p_avg - p)  # Step 2: Area normal sum

    sumA_norm = np.linalg.norm(sumA)
    if sumA_norm == 0:
        raise ValueError("Degenerate face: area normal is zero.")
    sumA_hat = sumA / sumA_norm  # Step 3: Normalize area normal

    sumAn = 0.0
    sumAnc = np.zeros(3)
    for i in range(N):
        p = points[i]
        p_next = points[(i + 1) % N]
        a = np.cross(p_next - p, p_avg - p)
        c = (p + p_next + p_avg) / 3.0  # Triangle center
        an = np.dot(a, sumA_hat)  # Projected area
        print(p,p_next,p_avg)
        print(np.linalg.norm(a),an)
        sumAn += an
        sumAnc += an * c

    face_center = sumAnc / sumAn  # Final centroid
    return face_center

def compute_edge_midpoints(face_points):
    """
    Compute the midpoints of the edges of a face.
    Args:
        face_points (list of list): List of points defining the face.
    Returns:
        list of np.array: List of midpoints for each edge.
    """
    points = np.array(face_points)
    midpoints = []
    N = len(points)
    for i in range(N):
        p = points[i]
        p_next = points[(i + 1) % N]
        midpoint = (p + p_next) / 2.0
        midpoints.append(midpoint)
    return midpoints

def set_axes_equal(ax):
    """
    Set equal scaling for all axes in a 3D plot.
    This ensures that the x, y, and z axes have the same scale.
    """
    limits = np.array([
        ax.get_xlim3d(),
        ax.get_ylim3d(),
        ax.get_zlim3d(),
    ])
    span = limits[:, 1] - limits[:, 0]
    center = np.mean(limits, axis=1)
    radius = 0.5 * max(span)
    
    ax.set_xlim3d([center[0] - radius, center[0] + radius])
    ax.set_ylim3d([center[1] - radius, center[1] + radius])
    ax.set_zlim3d([center[2] - radius, center[2] + radius])


def plot_face_with_centroid_and_midpoints(face_points):
    """
    Plot a 3D face, its centroid, edge midpoints, and lines connecting midpoints to the centroid.
    Args:
        face_points (list of list): List of points defining the face.
    """
    face_points = np.array(face_points)  # Ensure points are a NumPy array

    p_avg = np.mean(face_points, axis=0)  # Step 1: Geometric average of vertices
    print(p_avg)

    face_center = calculate_face_center(face_points)
    edge_midpoints = compute_edge_midpoints(face_points)

    # Extract face edges and vertices for plotting
    x = np.append(face_points[:, 0], face_points[0, 0])  # Close the face loop
    y = np.append(face_points[:, 1], face_points[0, 1])
    z = np.append(face_points[:, 2], face_points[0, 2])

    # Initialize 3D plot
    fig = plt.figure()
    ax = fig.add_subplot(111, projection='3d')

    # Plot the face edges
    ax.plot(x, y, z, color='blue', linewidth=2, label='Face Edges')

    # Plot the face edges
    ax.scatter(p_avg[0],p_avg[1],p_avg[2],color='black', s=30, label='p_avg')


    # Plot the face vertices
    ax.scatter(face_points[:, 0], face_points[:, 1], face_points[:, 2],
               color='red', s=50, label='Face Vertices')

    # Plot the computed centroid
    ax.scatter(face_center[0], face_center[1], face_center[2],
               color='green', s=100, label='Face Center')

    # Plot the midpoints of edges
    edge_midpoints = np.array(edge_midpoints)
    ax.scatter(edge_midpoints[:, 0], edge_midpoints[:, 1], edge_midpoints[:, 2],
               color='cyan', s=75, label='Edge Midpoints')

    # Plot lines connecting edge midpoints to the centroid
    for midpoint in edge_midpoints:
        ax.plot([midpoint[0], face_center[0]],
                [midpoint[1], face_center[1]],
                [midpoint[2], face_center[2]],
                color='purple', linestyle='--', linewidth=1)

    # Set equal scaling for all axes
    set_axes_equal(ax)

    # Set plot labels and title
    ax.set_xlabel('X')
    ax.set_ylabel('Y')
    ax.set_zlabel('Z')
    ax.set_title('3D Face with Centroid and Edge Midpoints')

    # Add a legend
    ax.legend()

    # Show the plot
    plt.show()

if __name__ == "__main__":
    # Example: Define a quadrilateral face with slight non-planarity
    face_points = [
        [0.0, 0.0, 0.0],
        [0.0,  0.5, 0.0],
        [10.0, 0.6, 0.0],
        [10.0, -0.1, 0.0]
#        [-1365.22, 361.362, 181.82],
#        [-1366.40,  361.782, 180.357],
#        [-1350.43, 361.919, 165.43],
#        [-1348.90, 361.541, 167.321]
    ]

    # Call the plotting function
    plot_face_with_centroid_and_midpoints(face_points)