import FreeCAD
import math


def Triangulate(points, MaxlengthLE = 8000, MaxAngleLE = math.pi, use3d=True):
    import numpy as np
    from scipy.spatial import Delaunay
    from stl import mesh as stlmesh
    import Mesh

    if points.__class__ is list:
        points = np.array(points)

    tri = Delaunay(points[:, :2])
    faces = tri.simplices
    wireframe = stlmesh.Mesh(np.zeros(faces.shape[0], dtype=stlmesh.Mesh.dtype))

    for i, f in enumerate(faces):
        if MaxLength(points[f[0]], points[f[1]], points[f[2]], MaxlengthLE) and \
           MaxAngle(points[f[0]], points[f[1]], points[f[2]], MaxAngleLE):
            for j in range(3):
                wireframe.vectors[i][j] = points[f[j], :]
                if not use3d:
                    wireframe.vectors[i][j][2] = 0

    MeshObject = Mesh.Mesh(wireframe.vectors.tolist())
    if len(MeshObject.Facets) == 0:
        return None
    MeshObject.harmonizeNormals()
    if MeshObject.Facets[0].Normal.z < 0:
        MeshObject.flipNormals()
    return MeshObject


def MaxLength(P1, P2, P3, MaxlengthLE):
    """ Calculation of the 2D length between triangle edges """
    p1 = FreeCAD.Vector(P1[0], P1[1], 0)
    p2 = FreeCAD.Vector(P2[0], P2[1], 0)
    p3 = FreeCAD.Vector(P3[0], P3[1], 0)

    List = [[p1, p2], [p2, p3], [p3, p1]]
    for i, j in List:
        vec = i.sub(j)
        if vec.Length > MaxlengthLE:
            return False
    return True


def MaxAngle(P1, P2, P3, MaxAngleLE):
    """ Calculation of the 2D angle between triangle edges """
    p1 = FreeCAD.Vector(P1[0], P1[1], 0)
    p2 = FreeCAD.Vector(P2[0], P2[1], 0)
    p3 = FreeCAD.Vector(P3[0], P3[1], 0)
    List = [[p1, p2, p3], [p2, p3, p1], [p3, p1, p2]]
    for j, k, l in List:
        vec1 = j.sub(k)
        vec2 = l.sub(k)
        radian = vec1.getAngle(vec2)
        if radian > MaxAngleLE:
            return False
    return True


# prueba para ver si es mejor:
def Open3DTriangle(point_cloud):
    import numpy as np
    import open3d as o3d

    '''
    input_path = "your_path_to_file/"
    output_path = "your_path_to_output_folder/"
    dataname = "sample.xyz"
    point_cloud = np.loadtxt(input_path + dataname, skiprows=1)
    '''

    pcd = o3d.geometry.PointCloud()
    pcd.points = o3d.utility.Vector3dVector(point_cloud)

    pcd.normals = o3d.utility.Vector3dVector(np.zeros((1, 3)))
    pcd.estimate_normals()
    pcd.orient_normals_consistent_tangent_plane(100)

    mesh, densities = o3d.geometry.TriangleMesh.create_from_point_cloud_poisson(pcd,
                                                                             depth=8,
                                                                             width=0,
                                                                             scale=1.1,
                                                                             linear_fit=False,
                                                                             n_threads=8)
    o3d.visualization.draw_geometries([mesh])
    #bbox = pcd.get_axis_aligned_bounding_box()
    #p_mesh_crop = mesh.crop(bbox)
    return mesh