def testSquare():

    ptlist = {
        "vertices": np.array(
            ((0.0, 0.0), (0.5, 0.0), (1.0, 0.0), (0.0, 0.5), (0.5, 0.5), (1.0, 0.5), (0.0, 1.0), (0.5, 1.0), (1.0, 1.0))
        )
    }
    t = triangle.triangulate(ptlist)
    t1 = triangle.triangulate(ptlist, "qa0.001")

    triangle.plot.compare(plt, t, t1)
    #    plt.show()

    L, M = FE.assembleMatrices(t)
    #    print L
    #    print '\n\n'
    #    print M

    np.savetxt("textL", L)
    np.savetxt("textM", M)

    eig = FE.eigenvalues(L, M)
    elist = eig[0]
    efunc = eig[1]
    print elist[0]
    print elist[1]
    print elist[2]

    #    vertices = np.asarray(t['vertices'])
    #    faces = np.asarray(t['triangles'])
    #    x = vertices[:,0]
    #    y = vertices[:,1]

    #    z = efunc[1]

    #    plt.figure()
    #    plt.tricontourf(x,y,faces,z,cmap='afmhot')
    #    plt.show()
    print "****************************"

    L, M = FE.assembleMatrices(t1)
    eig = FE.eigenvalues(L, M)
    elist = eig[0]
    efunc = eig[1]
    for j in range(10):
        print elist[j]

    vertices = np.asarray(t1["vertices"])
    faces = np.asarray(t1["triangles"])
    x = vertices[:, 0]
    y = vertices[:, 1]
    z = efunc[:, 5]

    plt.figure()
    plt.tricontourf(x, y, z, 100, cmap="afmhot")
    plt.show()

    print "***************************\n\n\n\n\n"

def testFE():
    ptlist = {"vertices": np.array(((0, 0), (1, 0), (1, 1), (0, 1)))}
    triang = triangle.triangulate(ptlist)

    print triang
    print "\n\n"
    print triang["vertices"]
    print triang["triangles"]

    triangle.plot.compare(plt, ptlist, triang)
    plt.show()

    print "\n\nNow testing the FE assembly ..."

    L, M = FE.assembleMatrices(triang)

    elist = FE.eigenvalues(L, M)[0]
    elist.sort()

    print "eigenvalues:"

    for j in elist:
        print j

    triangle.plot.compare(plt, ptlist, triang)
    plt.show()

    triang = triangle.triangulate(triang, "rqa0.1")

    L, M = FE.assembleMatrices(triang)

    elist = FE.eigenvalues(L, M)[0]
    elist.sort()

    print "\n\neigenvalues:"

    for j in elist:
        print j

    triangle.plot.compare(plt, ptlist, triang)
    plt.show()

    triang = triangle.triangulate(triang, "rqa0.01")

    L, M = FE.assembleMatrices(triang)

    elist = FE.eigenvalues(L, M)[0]
    elist.sort()

    print "\n\neigenvalues:"

    for j in elist:
        print j

    triangle.plot.compare(plt, ptlist, triang)
    plt.show()

 def draw_triangulated_rectangle(self, width, height, pos, octaves, persistance, scale, size):
     x, y = pos
     all_verts = [(x, y), (x, y + height), (x + width, y + height), (x + width, y)]
     A = {"vertices": array(all_verts)}
     command = "cqa" + size
     B = triangle.triangulate(A, command)
     tri_indices = B["triangles"]
     new_triangles = []
     new_vertices = []
     tri_verts = B["vertices"]
     nv_ap = new_vertices.append
     new_ap = new_triangles.append
     tri_count = 0
     for tri in tri_indices:
         new_ap((tri[0], tri[1], tri[2]))
         tri_count += 1
     vert_count = 0
     for tvert in tri_verts:
         nv_ap([tvert[0], tvert[1], octaves, persistance, scale])
         vert_count += 1
     print vert_count, tri_count
     return {
         "triangles": new_triangles,
         "vertices": new_vertices,
         "vert_count": vert_count,
         "tri_count": tri_count,
         "vert_data_count": 5,
     }

def triangleMesh(x,y,refinement):
	

	domain = {'vertices':np.array([[0.,0.],[1.,0.],[x,y]]),'triangles':np.array([[0,1,2]])}
	mesh = triangle.triangulate(domain, 'qa'+str(refinement))

	return mesh

def triangulate(P):
    n = len(P)
    S = np.repeat(np.arange(n+1),2)[1:-1]
    S[-2:] = n-1,0
    S = S.reshape((-1, 2))
    T = triangle.triangulate({'vertices': P[:,:2], 'segments': S}, "p")
    return  T["triangles"].ravel()

def triangulate_regular_polygon(sides, radius, pos, size):
        x, y = pos
        angle = 2 * pi / sides
        all_verts = []
        all_verts_a = all_verts.append
        r = radius
        for s in range(sides):
            new_pos = x + r * sin(s * angle), y + r * cos(s * angle)
            all_verts_a(new_pos)
        A = {'vertices':array(all_verts)}
        command = 'cqa' + size + 'YY'
        B = triangle.triangulate(A, command)
        tri_indices = B['triangles']
        new_indices = []
        new_vertices = {}
        tri_verts = B['vertices']
        new_ex = new_indices.extend
        ind_count = 0
        for tri in tri_indices:
            new_ex((tri[0], tri[1], tri[2]))
            ind_count += 3
        vert_count = 0
        for i, tvert in enumerate(tri_verts):
            new_vertices[i] = {
                'pos': [tvert[0], tvert[1]], 
                'v_color': [255, 255, 255, 255]
                }
            vert_count += 1
        return {'indices': new_indices, 'vertices': new_vertices, 
            'vert_count': vert_count, 'ind_count': ind_count}

    def _triangulate_raster_from_file(self):
        """
        Main function used to create the irregular TIN surface for Badlands based on
        a regular DEM file.
        """

        # Compute DEM edges
        self._raster_edges()

        # Compute TIN grid boundaries
        self._TIN_ghosts_bounds()

        # Create TIN
        tinPts = numpy.vstack(( self.bounds, self.edges))
        self.tinMesh = triangle.triangulate( dict(vertices=tinPts),'Dqa'+str(self.areaDel))
        ptsTIN = self.tinMesh['vertices']

        # Check extent
        checkXmin = ptsTIN[self.boundsPt:,0].min()
        checkXmax = ptsTIN[self.boundsPt:,0].max()
        checkYmin = ptsTIN[self.boundsPt:,1].min()
        checkYmax = ptsTIN[self.boundsPt:,1].max()

        if checkXmin < self.rectX.min() or checkXmax > self.rectX.max():
            raise ValueError('Error in defining the X boundary nodes, you will need to adjust the resolution value')
        if checkYmin < self.rectY.min() or checkYmax > self.rectY.max():
            raise ValueError('Error in defining the Y boundary nodes, you will need to adjust the resolution value')

        # Add masks
        self.bmask = numpy.zeros(len(ptsTIN[:,0]))
        self.bmask[:self.boundsPt] = 1

        return

	def curve_to_mesh(self,curve):
		M=len(curve)
		segs = np.zeros((M,2)).astype(np.int32)
		segs[:,0] = np.linspace(0,M-1,M).astype(np.int32)
		segs[:,1] = segs[:,0]+1
		segs[-1,-1] = 0
		
		face={"vertices": np.array(curve).astype(np.float32),"segments": np.array(segs).astype(np.int32)}
		
		tri = triangle.triangulate(face,'pq10')
		
		verts_2d     = tri["vertices"]
		trinagles_2d = tri["triangles"]
		
		verts     = np.zeros((len(verts_2d),4),dtype=np.float32)
		trinagles = np.zeros((len(trinagles_2d),3),dtype=np.float32)
		
		
		verts[:,0]     = verts_2d[:,0]
		trinagles[:,0] = trinagles_2d[:,0]
		verts[:,1]     = verts_2d[:,1]
		trinagles[:,1] = trinagles_2d[:,1]
		trinagles[:,2] = trinagles_2d[:,2]
		
		return GeoObject(verts=verts,tris=trinagles)

 def draw_triangulated_regular_polygon(self, sides, radius, pos, octaves, persistance, scale, size):
     x, y = pos
     angle = 2 * pi / sides
     all_verts = []
     all_verts_a = all_verts.append
     r = radius
     for s in range(sides):
         new_pos = x + r * sin(s * angle), y + r * cos(s * angle)
         all_verts_a(new_pos)
     A = {"vertices": array(all_verts)}
     command = "cqa" + size + "YY"
     B = triangle.triangulate(A, command)
     tri_indices = B["triangles"]
     new_triangles = []
     new_vertices = []
     tri_verts = B["vertices"]
     nv_ap = new_vertices.append
     new_ap = new_triangles.append
     tri_count = 0
     for tri in tri_indices:
         new_ap((tri[0], tri[1], tri[2]))
         tri_count += 1
     vert_count = 0
     for tvert in tri_verts:
         nv_ap([tvert[0], tvert[1], octaves, persistance, scale])
         vert_count += 1
     return {
         "triangles": new_triangles,
         "vertices": new_vertices,
         "vert_count": vert_count,
         "tri_count": tri_count,
         "vert_data_count": 5,
     }

def STLWrite(faces, filename, thickness=0):
  import triangle

  shape = None
  shells = []
  triangles = []
  for f in faces:
    r = f[0]
    A = f[1]

    facets = []
    B = triangle.triangulate(A, opts='p')
    if not 'triangles' in B:
      print "No triangles in " + B
      continue

    if thickness:
      for t in [np.transpose(np.array([list(B['vertices'][x]) + [0,1] for x in (face[0], face[1], face[2])])) for face in B['triangles']]:
        facets.extend([np.dot(r, x) for x in inflate(t, thickness=thickness)])
      for t in [np.transpose(np.array([list(A['vertices'][x]) + [0,1] for x in (edge[0], edge[1])])) for edge in A['segments']]:
        facets.extend([np.dot(r, x) for x in inflate(t, thickness=thickness, edges=True)])
    else:
      for t in [np.transpose(np.array([list(B['vertices'][x]) + [0,1] for x in (face[0], face[1], face[2])])) for face in B['triangles']]:
        facets.append(np.dot(r, t))

    triangles.extend(facets)

    if thickness:
      FREECADPATH = '/usr/lib64/freecad/lib'
      import sys
      sys.path.append(FREECADPATH)
      import FreeCAD
      import Part
      meshes = []
      for f in (np.transpose(t[0:3,:]) for t in facets):
        try:
          meshes.append(Part.Face(Part.Wire([Part.makeLine(tuple(f[x]), tuple(f[x-1])) for x in range(3)])))
        except RuntimeError:
          print "Skipping face: " + repr(f)
      shell = Part.makeShell(meshes)
      shells.append(shell)
      if shape is None:
        shape = shell
      else:
        shape = shape.fuse(shell)

  if shape:
    with open("freecad" + filename, 'wb') as fp:
      shape.exportStl("freecad" + filename)

  from stlwriter import Binary_STL_Writer
  faces = triangles

  with open(filename, 'wb') as fp:
    writer = Binary_STL_Writer(fp)
    writer.add_faces(faces)
    writer.close()

def triangulate(geometry, max_area=None):
    """Use the triangle library to triangulate a polygon

    Args:
        polygon (shapely.*): shapely geometry representing the area to
            triangulate
        max_area (float): If provided, the triangulation will be refined
            until all triangles have area less than this maximum.

    Returns:
        list: list of triangular polygons
    """
    if not geometry.is_valid:
        raise ValueError("Tried to triangulate invalid geometry", geometry)

    if hasattr(geometry, "geoms"): # polygon is a MultiPolygon
        polygons = list(geometry.geoms)
    else:
        polygons = [geometry]

    vertices = []
    segments = []
    for polygon in polygons:
        offset = len(vertices)
        vertices.extend(polygon.exterior.coords[0:-1])
        segments.extend(circular_pairs(
            range(offset, offset+len(polygon.exterior.coords)-1)))
        for ring in polygon.interiors:
            offset = len(vertices)
            vertices.extend(ring.coords[0:-1])
            segments.extend(circular_pairs(
                range(offset, offset+len(ring.coords)-1)))

    shape = {'vertices': numpy.array(vertices),
             'segments': numpy.array(segments, dtype=numpy.int32)}

    # Find the holes in the geometry
    buffer_by = numpy.sqrt(geometry.envelope.area)
    complement = geometry.envelope.buffer(
        buffer_by, cap_style=2, join_style=2).difference(geometry)
    if complement.geom_type == "MultiPolygon":
        shape['holes'] = numpy.array([interior.representative_point().coords[0]
                                      for interior in complement.geoms])
    elif complement.geom_type == "Polygon":
        shape['holes'] = numpy.array(complement.representative_point().coords[0])

    if max_area is None:
        opts = "p"
    else:
        opts = "pa{}".format(max_area)

    triangulation = triangle.triangulate(shape, opts)
    return [shapely.geometry.Polygon([triangulation['vertices'][i]
                                      for i in triplet])
            for triplet in triangulation['triangles']]

def RMS_calc(lons,lats,rotation):
    #Define an equal area projection around the plate
    m = pyproj.Proj("+proj=aea +lat_1="+`min(lats)`+" +lat_2="+`max(lats)`+" +lat_0="+`(min(lats)+max(lats))/2`+" +lon_0="+`(min(lons)+max(lons))/2`)
    #Create irregular triangular mesh for the plate polygon
    data={}
    data['vertices']=np.column_stack((lons,lats))
    segs=[[0,len(lons)-1]]
    for i in np.arange(0,len(lons)-1): segs.append([i,i+1])
    data['segments']=np.array(segs)
    t = triangulate(data, 'pa50q30') #starting off with too small an area can cause crashes
    #can refine further using r switch - note I do so right now but the difference in the overall result is small
    t2= triangulate(t, 'ra10q30')
    f, (ax1, ax2) = plt.subplots(1, 2, sharey=True)
    tplot.plot(ax1, **t)
    plt.title('1st Triangulation - min area 50, min angle 30')
    tplot.plot(ax2, **t2)
    plt.title('Refined mesh - min area 10, min angle 30')
    plt.tight_layout()
    units=t2['vertices'][t2['triangles']]
    vel_lat,vel_lon,vel_mag,vel_az= [],[],[],[]
    totvelsquared=0.
    totarea=0.
    for unit in units:
        tlon,tlat=m(unit[:,0],unit[:,1])
        thing=Polygon(np.column_stack((tlon,tlat)))
        area=thing.area/1e6
        x,y=thing.centroid.xy
        x,y=m(x,y,inverse=True)
        platevel=get_plate_velocity(([y[0],x[0]]),rotation)
        totvelsquared=totvelsquared+(area*platevel[0]**2)
        totarea=totarea+area
        vel_lon.append(x)
        vel_lat.append(y)
        vel_mag.append(platevel[0])
        vel_az.append(platevel[1])
    rms=np.sqrt(totvelsquared/totarea)
    velgrid=pd.DataFrame(np.column_stack((vel_lon,vel_lat,vel_mag,vel_az)), columns=['Lons','Lats','Rate','Azimuth'])
    return rms, totarea, velgrid

def cycles2triangles(polygon):
    triangleSet = []

    vertices = []
    stringVertices = set()
    for cycle in polygon:
        for vertex in cycle:
            stringVertex = str(vertex)
            if (stringVertex not in stringVertices):
                vertices.append(vertex)
                stringVertices.add(stringVertex)

    edges = []
    externalCycle = polygon[0]
    edges += face2edge([[vertices.index(vertex) for vertex in externalCycle]])

    holeEdges = []
    holeInnerPoints = []
    internalCycles = polygon[1:]
    for hole in internalCycles:
        holeEdges += face2edge([[vertices.index(vertex) for vertex in hole]])
        holeInnerPoints.append(internalTo(hole,holeEdges))
        edges += holeEdges

    if holeInnerPoints != []:
        triangulation = triangulate({"vertices":vertices,"segments":edges,"holes":holeInnerPoints},'p')
    else:
        triangulation = triangulate({"vertices": vertices, "segments": edges}, 'p')

    vertices = triangulation['vertices'].tolist()
    triangles = triangulation['triangles'].tolist()

    trias = [[vertices[index] for index in triangle] for triangle in triangles]
    trias = [[[vertex[0], vertex[1], 0.0] for vertex in tri] for tri in trias]

    triangleSet += [trias]
    
    return triangleSet

def foo3():
    import triangle
    import triangle.plot as plot

    node_list, crease_list, crease_types = load_creasepattern('test.creasepattern')
    #print node_list
    #print crease_list

    paper = {}
    paper['vertices'] = node_list 
    paper['segments'] = crease_list

    ax1 = mpl.subplot(121, aspect='equal')
    plot.plot(ax1, **paper)
    
    t = triangle.triangulate(paper, 'p')
    print t

    ax2 = mpl.subplot(122, sharex=ax1, sharey=ax1)
    plot.plot(ax2, **t)

    nodes = t['vertices']
    triangles = t['triangles']
    #offset = 0.01
    for i in range(triangles.shape[0]):
        mean_x = np.mean(nodes[triangles[i,:],0])
        mean_y = np.mean(nodes[triangles[i,:],1])
        mpl.text(mean_x, mean_y, '%d' % i)

    edge2triangle = get_edge2triangle(t['triangles'])
    print 'edge2triangle'
    print edge2triangle

    neighbors, neighbor_angles = get_neighbors(node_list, crease_list) 
    i = 4
    angle = 15
    crease_angles = [angle, 180, angle, None, angle, 180, angle, None]
    ans = solve_node(neighbor_angles[i], crease_angles)
    crease_angles = ans[0]
    known_creases = {}
    known_creases = add_node_creases(known_creases, i, neighbors[i], crease_angles)
    known_creases = add_flat_creases(known_creases, triangles)
    print known_creases

    frames, nodes3d = propagate_frames(nodes, triangles, known_creases, triangle_index=0)
    for i, n in enumerate(nodes3d):
        print i, n

    plot_creasepattern(nodes, crease_list, crease_types=crease_types, triangles=triangles)

def triangulate(vertices):
    n = len(vertices)
    vertices = np.array(vertices)
    zmean = vertices[:,2].mean()
    vertices_2d = vertices[:,:2]
    segments = np.repeat(np.arange(n+1),2)[1:-1]
    segments[-2:] = n-1,0
    T = triangle.triangulate({'vertices': vertices_2d,
                              'segments': segments}, "p")
    vertices_2d = T["vertices"]
    triangles   = T["triangles"]
    vertices = np.empty((len(vertices_2d),3))
    vertices[:,:2] = vertices_2d
    vertices[:,2] = zmean
    return vertices, triangles

    def Triangulate(self):
        delaunay = triangle.triangulate({'vertices' : self.vertices})
        self.segments_vertices = {}
        self.vertices_segments = {}
        self.delaunay_triangles = {}

        for tri in delaunay['triangles']:
            for ind in range(3):
                a, b, c = tri[ind], tri[(ind + 1) % 3], tri[(ind + 2) % 3]
                self.InsertSegments_Vertices(a, b, c)
                self.InsertVertices_Segments(a, b, c)
        
        for tri in delaunay['triangles']:
            a, b, c = sorted((tri[0], tri[1], tri[2]))
            self.delaunay_triangles[(a, b, c)] = True

def foo2():
    # From http://dzhelil.info/triangle/delaunay.html
    # (not my code)
    import triangle
    import triangle.plot as plot

    face = triangle.get_data('face')
    print face

    ax1 = mpl.subplot(121, aspect='equal')
    plot.plot(ax1, **face)

    t = triangle.triangulate(face, 'p')

    ax2 = mpl.subplot(122, sharex=ax1, sharey=ax1)
    triangle.plot.plot(ax2, **t)

    mpl.show()

def triangulate(polygon):
    """
    Triangulates 3D polygons
    """
    vect1 = [polygon[1][0] - polygon[0][0],
             polygon[1][1] - polygon[0][1],
             polygon[1][2] - polygon[0][2]]
    vect2 = [polygon[2][0] - polygon[0][0],
             polygon[2][1] - polygon[0][1],
             polygon[2][2] - polygon[0][2]]
    vectProd = [vect1[1] * vect2[2] - vect1[2] * vect2[1],
                vect1[2] * vect2[0] - vect1[0] * vect2[2],
                vect1[0] * vect2[1] - vect1[1] * vect2[0]]
    polygon2D = []
    segments = list(range(len(polygon)))
    segments.append(0)
    # triangulation of the polygon projected on planes (xy) (zx) or (yz)
    if(math.fabs(vectProd[0]) > math.fabs(vectProd[1]) and math.fabs(vectProd[0]) > math.fabs(vectProd[2])):
        # (yz) projection
        for v in range(0,len(polygon)):
            polygon2D.append([polygon[v][1], polygon[v][2]])
    elif(math.fabs(vectProd[1]) > math.fabs(vectProd[2])):
        # (zx) projection
        for v in range(0,len(polygon)):
            polygon2D.append([polygon[v][0], polygon[v][2]])
    else:
        # (xy) projextion
        for v in range(0,len(polygon)):
            polygon2D.append([polygon[v][0], polygon[v][1]])

    triangulation = triangle.triangulate({'vertices': polygon2D, 'segments': segments})
    if 'triangles' not in triangulation:    # if polygon is degenerate
        return []
    trianglesIdx = triangulation['triangles']
    triangles = []

    for t in trianglesIdx:
        # triangulation may break triangle orientation, test it before adding triangles
        if(t[0] > t[1] > t[2] or t[2] > t[0] > t[1] or t[1] > t[2] > t[0]):
            triangles.append([polygon[t[1]], polygon[t[0]],polygon[t[2]]])
        else:
            triangles.append([polygon[t[0]], polygon[t[1]],polygon[t[2]]])

    return triangles

def triangulate(game_objects, scenario_width, scenario_height):
    points = []
    segments = []
    holes = []

    for obj in game_objects:
        if obj.getRole() == GOBJ.MAN:
            continue
        offset = len(points)
        vertices = obj.getPosScr()
        points = points + vertices

        #if obj.getRole() == GOBJ.IMMOVEABLE:

        hole = ((vertices[0][0] + vertices[2][0])/2, (vertices[0][1] + vertices[2][1])/2)
        holes.append(hole)
        edges = obj.getEdges()
        edges = [ [edge[0] + offset, edge[1] + offset] for edge in edges]
        segments = segments + edges

    ## add frames 
    points = points + [[0,0],[0,scenario_height],[scenario_width,scenario_height],[scenario_width,0]]
    points = np.array(points)
    segments = np.array(segments)
    holes = np.array(holes)

    dic = {}
    dic["vertices"] = points
    dic["segments"] = segments
    dic["holes"] = holes


    tri = triangle.triangulate(dic,'pc')

    
    zones = [Polygon(tri["vertices"][t]) for t in tri["triangles"]]
    graph = createGraph(tri["triangles"], tri["vertices"], tri["segments"])

    #graph = create_graph_advanced(tri["triangles"], tri["vertices"], tri["segments"])
    return graph, zones

	def refineMesh(self, args):
		"""
		See the API <http://dzhelil.info/triangle> for information on optional arguments.
		Common arguments (use any combination):
			p - triangulates a Planar Straight Line Graph.
			a - imposes a maximum area for each triangle.
			q - quality mesh generation no angles smaller than specified degrees (default: 20).
			c - encloses convex hull with line segments
		"""
		t = clock()
		args = str(args) #ensure string
		tri = triangle.triangulate(self.meshDict, args)

		self.x = tri['vertices'][:,0]
		self.y = tri['vertices'][:,1]
		self.simplicies = tri['triangles']
		self.centroids = (tri['vertices'][tri['triangles'][:,0]] + tri['vertices'][tri['triangles'][:,1]] + tri['vertices'][tri['triangles'][:,2]])/3
		self.bmask = self.boundary_mask(tri)

		if self.verbose:
			print " - Mesh refinement complete. %i vertices in %f secs with arguments '%s'" % (len(tri['vertices']), clock()-t, args)

		return