#!/usr/bin/python3
import lxml.etree, math, sys

_, inpath, outpath, basename = sys.argv

tree = lxml.etree.parse(inpath)

polys = []
edge2poly = {}

# Polygons must be convex to work correctly, but we don't verify that

def points_to_edges(points):
    points = points + [points[0]]
    return zip(points[:-1], points[1:])

for polygon in tree.findall("objectgroup[@name='navmesh']/object/polygon"):
    basex = int(polygon.getparent().attrib["x"])
    basey = int(polygon.getparent().attrib["y"])
    points=[]
    for point in polygon.attrib["points"].split(" "):
        x,y=point.split(",")
        x,y=int(x)+basex,int(y)+basey
        points.append((x,y))
    #print(points)
    
    ax,ay=points[0]
    bx,by=points[1]
    cx,cy=points[2]
    abx,aby=bx-ax,by-ay
    acx,acy=cx-ax,cy-ay
    cross = abx*acy-acx*aby
    # positive for clockwise winding order. Example clockwise winding order: (0,0), (1,0), (0,1)
    if cross < 0:
        points = points[::-1]
    
    poly_id = len(polys)
    polys.append(points)
    
    for edge in points_to_edges(points):
        edge_id = tuple(sorted(edge))
        if edge_id not in edge2poly: edge2poly[edge_id] = []
        edge2poly[edge_id].append(poly_id)
        assert len(edge2poly[edge_id]) <= 2, "more than 2 polygons share edge "+str(edge_id)

pathfind_edges = [[] for _ in range(len(polys))]
for edgepolys in edge2poly.values():
    assert len(edgepolys) in (1,2)
    if len(edgepolys) == 1: continue
    p1,p2 = edgepolys
    pathfind_edges[p1].append(p2)
    pathfind_edges[p2].append(p1)

pathfind_matrix = [[255 for _1 in range(len(polys))] for _2 in range(len(polys))]
# pathfind_matrix[source][dest] is how to get to dest from source

for dest in range(len(polys)):
    # Find paths to poly by breadth-first search. We don't take the size of the polygon into account yet.
    openset = [dest]
    while len(openset)>0:
        cur, openset = openset[0], openset[1:]
        for adjacent in pathfind_edges[cur]:
            if adjacent != dest and pathfind_matrix[adjacent][dest] == 255:
                pathfind_matrix[adjacent][dest] = cur
                openset.append(adjacent)



out = open(outpath,"w")

out.write("#include \"compiled_structures.h\"\n")
out.write(f"static const struct navmesh_tri {basename}_triangles[{len(polys)}] = {{\n");
for poly_id,points in enumerate(polys):
    out.write("\t{\n")
    out.write("\t\t"+str(len(points))+",\n")
    out.write("\t\t(const struct navmesh_tri_edge[]){\n")
    for a,b in points_to_edges(points):
        (ax,ay),(bx,by) = a,b
        # edge a-b and c is the other point
        # Line equation Px+Qy+R=0 from https://math.stackexchange.com/questions/422602/convert-two-points-to-line-eq-ax-by-c-0
        P,Q,R=ay-by,bx-ax,ax*by-bx*ay
        # Line equation >0 for points inside the triangle.
        # Normalize so that P^2+Q^2=1 so that the equation result tells us the distance. This can be used to find the closest triangle to a point.
        norm=1/math.sqrt(P*P+Q*Q)
        P *= norm
        Q *= norm
        R *= norm
        
        edge_id = tuple(sorted((a,b)))
        polys_on_edge = edge2poly[edge_id]
        assert len(polys_on_edge) in (1,2)
        assert poly_id in polys_on_edge
        if len(polys_on_edge) == 2:
            other_poly_id = [x for x in polys_on_edge if x!=poly_id][0]
        else:
            other_poly_id = -1
        
        centx, centy = (ax+bx)/2, (ay+by)/2
        
        out.write(f"\t\t\t{{{P},{Q},{R},{other_poly_id},{{{int(centx)},{int(centy)}}}}},\n")
        
        
        
    out.write("\t\t},\n")
    out.write("\t\t(const struct navmesh_point[]){" + ",".join(f"{{{x},{y}}}" for x,y in points) + "},\n")
    out.write("\t},\n");
out.write("};\n")
out.write(f"static const unsigned char {basename}_pathfind[{len(polys)*len(polys)}] = {{\n")
for dest in range(len(polys)):
    out.write("\t")
    for source in range(len(polys)):
        out.write(str(pathfind_matrix[source][dest])+",")
    out.write("\n")
out.write("};\n")
out.write(f"extern const struct navmesh {basename} = {{{len(polys)}, {basename}_triangles, {basename}_pathfind}};\n")