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

_, inpath, outpath, basename = sys.argv

tree = lxml.etree.parse(inpath)

tris = []
edge2tri = {}

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[0],points[2],points[1]
        bx,by,cx,cy=cx,cy,bx,by
        cross = -cross
    
    assert len(points)==3, "triangles only"
    tri_id = len(tris)
    tris.append(points)
    
    for edge in [(points[0],points[1]), (points[1],points[2]), (points[2],points[0])]:
        edge_id = tuple(sorted(edge))
        if edge_id not in edge2tri: edge2tri[edge_id] = []
        edge2tri[edge_id].append(tri_id)
        assert len(edge2tri[edge_id]) <= 2, "more than 2 triangles share edge "+str(edge_id)

pathfind_edges = [[] for _ in range(len(tris))]
for edgetris in edge2tri.values():
    assert len(edgetris) in (1,2)
    if len(edgetris) == 1: continue
    t1,t2 = edgetris
    pathfind_edges[t1].append(t2)
    pathfind_edges[t2].append(t1)

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

for dest in range(len(tris)):
    # Find paths to tri by breadth-first search. We don't take the size of the triangle 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(tris)}] = {{\n");
for tri_id,points in enumerate(tris):
    out.write("\t{\n\t\t{\n");
    for a,b,c in [(points[0],points[1],points[2]), (points[1],points[2],points[0]), (points[2],points[0],points[1])]:
        (ax,ay),(bx,by),(cx,cy) = a,b,c
        # 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)))
        tris_on_edge = edge2tri[edge_id]
        assert len(tris_on_edge) in (1,2)
        assert tri_id in tris_on_edge
        if len(tris_on_edge) == 2:
            other_tri_id = [x for x in tris_on_edge if x!=tri_id][0]
        else:
            other_tri_id = -1
        
        centx, centy = (ax+bx)/2, (ay+by)/2
        
        out.write(f"\t\t\t{{{P},{Q},{R},{other_tri_id},{{{int(centx)},{int(centy)}}}}},\n")
        
        
        
    out.write("\t\t},\n")
    out.write("\t\t{" + ",".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(tris)*len(tris)}] = {{\n")
for dest in range(len(tris)):
    out.write("\t")
    for source in range(len(tris)):
        out.write(str(pathfind_matrix[source][dest])+",")
    out.write("\n")
out.write("};\n")
out.write(f"extern const struct navmesh {basename} = {{{len(tris)}, {basename}_triangles, {basename}_pathfind}};\n")