(* Created 1993 by Rober M. Rosi *)

MODULE Triang;

IMPORT 
  Oct, R3, LR3, LR3Extras, FileRd, FileWr, Random, Math, Thread, 
  Rd, Wr, Fmt, Lex, FloatMode, OSError, Text, TextRd, TextWr;
FROM Oct IMPORT 
  RBits, RotBits, Sym, Onext, Lnext, Rprev, Rot, Tor, Flip, Oprev, 
  Splice;
  
REVEAL
  Edge = PublicEdge BRANDED OBJECT
    org: ARRAY RBits OF Node;
  OVERRIDES
    init := EdgeInit;
  END;

REVEAL 
  Node = PublicNode BRANDED OBJECT 
    END;

REVEAL     
  Vertex = PublicVertex BRANDED OBJECT 
    END;

REVEAL 
  Face = PublicFace BRANDED OBJECT
    END;
    
(* === INIT METHODS === *)

PROCEDURE EdgeInit(e: Edge): Edge=
  BEGIN
    EVAL NARROW(e,Oct.Edge).init(); 
    e.org[0] := NIL; (* For now *)
    e.org[1] := NIL; (* For now *)
    e.org[2] := NIL; (* For now *)
    e.org[3] := NIL; (* For now *)
    RETURN  e;
  END EdgeInit;

(* === ELEMENT CREATION === *)
  
PROCEDURE MakeEdge(): Arc =
  BEGIN
    WITH e = NEW(Edge).init() DO
      RETURN Arc{edge := e, bits := 0};
    END;
  END MakeEdge;
  
PROCEDURE MakeVertex(): Vertex =
  BEGIN RETURN NEW(Vertex) END MakeVertex;

PROCEDURE MakeFace(): Face =
  BEGIN RETURN NEW(Face) END MakeFace;

(* === ARC PROPERTIES === *)

PROCEDURE Org(e: Arc): Node =
  BEGIN
    WITH c = NARROW(e.edge, Edge).org[RotBits(e)] DO
      RETURN c;
    END;
  END Org;

PROCEDURE SetOrg(e: Arc; n: Node) =
  BEGIN
    WITH c = NARROW(e.edge, Edge).org[RotBits(e)] DO
      c := n
    END;
  END SetOrg;

PROCEDURE SetAllOrgs(a: Arc; n: Node) =
  VAR t: Arc := a;
  BEGIN
    REPEAT
      SetOrg(t, n);
      t := Onext(t)
    UNTIL t = a
  END SetAllOrgs;

PROCEDURE Left(a: Arc): Node =
  BEGIN
    RETURN Org(Tor(a))
  END Left;
   
PROCEDURE SetLeft(e: Arc; n: Node) =
  BEGIN
    SetOrg(Tor(e), n)
  END SetLeft;

PROCEDURE SetAllLefts(a: Arc; n: Node) =
  VAR t: Arc := a;
  BEGIN
    REPEAT
      SetLeft(t, n);
      t := Lnext(t)
    UNTIL t = a
  END SetAllLefts;
  
PROCEDURE OrgV(a: Arc): Vertex =
  BEGIN
    RETURN NARROW(Org(a), Vertex)
  END OrgV;
   
PROCEDURE LeftF(a: Arc): Face =
  BEGIN
    RETURN NARROW(Org(Tor(a)), Face)
  END LeftF;
   
(* === CONSTRUCTION TOOLS === *)

PROCEDURE Connect(a, b: Arc): Arc =
  VAR e: Arc;
  BEGIN
    e := MakeEdge();
    Splice(e, a);
    Splice(Sym(e), b);
    RETURN e;
  END Connect;
  
PROCEDURE AddSpokes(a: Arc): Arc =
  VAR e, b, c, d: Arc;
  BEGIN
    WITH v = MakeVertex() DO 
      e := MakeEdge();
      Splice(e, a); 
      SetOrg(Sym(e), v);
      SetOrg(e, Org(a));

      b := a;
      d := e;
      c := Lnext(a);
      WHILE c # e DO
        WITH
          r = Connect(c, Sym(d)),
          f = MakeFace()
        DO
          SetOrg(r, Org(c));
          SetOrg(Sym(r), v);
          SetLeft(r, f);
          SetLeft(Sym(d), f);
          SetLeft(b, f);
          b := c;
          d := r;
          c := Lnext(c)
        END
      END;

      WITH
        f = MakeFace()
      DO
        SetLeft(e, f);
        SetLeft(Sym(d), f);
        SetLeft(b, f);
      END;

      RETURN e
    END
  END AddSpokes;

PROCEDURE MakeGrid(nx, ny: CARDINAL): ARRAY [0..7] OF Arc =
      
  VAR EdgeCount: CARDINAL := 0;
  
  PROCEDURE MakeCell(s: Arc): Arc =
    (* 
      Adds four triangles nested in the corner between "s" 
      (on the west side, pointing north) and "Oprev(s)" 
      (on the south side, pointing east).
      Returns the east edge of the new cell, pointing north. *)
    VAR b, c, t: Arc;
        v: Vertex;
    BEGIN
      v := MakeVertex();
      t := Sym(Oprev(s));

      b := MakeEdge(); b.edge.num := EdgeCount; INC(EdgeCount);
      Splice(Oprev(t), b);
      SetOrg(b, Org(t));
      SetOrg(Sym(b), v);
      
      c := MakeEdge(); c.edge.num := EdgeCount; INC(EdgeCount);
      Splice(c, Sym(b));
      Splice(Sym(s), Sym(c));
      SetOrg(c, v);
      SetOrg(Sym(c), Org(Sym(s)));

      EVAL AddSpokes(b);

      RETURN b
    END MakeCell;
  
  PROCEDURE MakeCellRow(a: Arc): Arc =
  (* 
    Adds a new row of cells, bounded on the west side by arc "a"
    (assumed to be pointing north).  Returns the east edge of the 
    easternmost cell, pointing north. *)
    BEGIN
      FOR col := 0 TO nx-1 DO
        a := MakeCell(a);
      END;
      RETURN a
    END MakeCellRow;

  VAR b, t: Arc;
      ca: ARRAY [0..7] OF Arc;
  BEGIN
    (* Create bottom row of edges: *)  
    FOR col := 0 TO nx-1 DO
      WITH c = MakeEdge() DO
        c.edge.num := EdgeCount; INC(EdgeCount);
        IF col = 0 THEN
          WITH v = MakeVertex() DO
            SetOrg(c, v);
          END;
          t := c;
        ELSE
          Splice(Sym(b), c);
          SetOrg(c, Org(Sym(b)));
        END;
        b := c;
        WITH w = MakeVertex() DO
          SetOrg(Sym(b), w)
        END;
      END
    END;

    ca[1] := Flip(t);
    ca[2] := Sym(b);
    FOR row := 0 TO ny-1 DO
      (* "t" is the first edge on the top of the last row of cells *)
      (* "b" is the last edge on the top of the last row of cells *)
      (* Both are pointing from left to right *)
      (* Add another row of cells, beginning just above "t": *)
      WITH a = MakeEdge() DO
        a.edge.num := EdgeCount; INC(EdgeCount);
        IF row = 0 THEN ca[0] := a END;
        IF row = ny-1 THEN ca[7] := Flip(Sym(a)) END;

        Splice(a, t);
        SetOrg(a, Org(t));
        WITH u = MakeVertex() DO
          SetOrg(Sym(a), u)
        END;
        WITH f = MakeCellRow(a) DO
          t := Oprev(Sym(a));
          b := Sym(Onext(Sym(f)));

          IF row = 0 THEN ca[3] := Flip(f) END;
          IF row = ny-1 THEN ca[4] := Sym(f) END;
        END
      END
    END;
    ca[6] := t;
    ca[5] := Flip(Sym(b));
    RETURN ca
  END MakeGrid;
  
PROCEDURE GridCell(a: Arc; READONLY x, y: CARDINAL): Arc =
  VAR c: Arc;
  BEGIN
    c := a;
    FOR i := 0 TO x-1 DO
      c := Oprev(Oprev(Oprev(Oprev(Sym(c)))))
    END;
    FOR j := 0 TO y-1 DO 
      c := Onext(Onext(Sym(Onext(Onext(c)))))
    END;
    RETURN c;
  END GridCell;
  
PROCEDURE EnumGridVertices(
    a: Arc; nx, ny: CARDINAL;
    visit: GridVisitProc;
  ) =
  VAR e: Arc;
  BEGIN
    FOR iy := 0 TO ny-1 DO 
      e := a;
      FOR ix := 0 TO nx-1 DO
        visit(e, 2*ix, 2*iy);
        visit(Sym(Onext(e)), 2*ix+1, 2*iy+1);
        IF iy = ny-1 THEN
          visit(Sym(Onext(Onext(e))), 2*ix, 2*iy+2)
        END;
        IF ix = nx-1 THEN
          visit(Sym(e), 2*ix+2, 2*iy)
        END;
        IF ix = nx-1 AND iy = ny-1 THEN 
          visit(Sym(Oprev(Oprev(Sym(e)))), 2*ix+2, 2*iy+2)
        END;
        IF ix < nx-1 THEN
          e := Oprev(Oprev(Oprev(Oprev(Sym(e)))))
        END
      END;
      IF iy < ny-1 THEN
        a := Onext(Onext(Sym(Onext(Onext(a)))))
      END
    END
  END EnumGridVertices;

PROCEDURE SetQuarterGridStyles(
    c: Arc; 
    order: CARDINAL; 
    vertexStyle: VertexStyle;
    edgeStyle: EdgeStyle;
    joinStyle: VertexStyle;
    faceStyle: FaceStyle;
    facePatch: PatchNum;
  ) =
   
  PROCEDURE SetCornerVertex(v: Vertex) =
    BEGIN
      v.style := vertexStyle;
    END SetCornerVertex;
  
  PROCEDURE SetQuarterEdge(a: Arc) =
    BEGIN
      FOR i := 0 TO order DO 
        IF i # 0 THEN 
          WITH v = OrgV(a) DO
            v.style := joinStyle
          END;
        END;
        WITH e = NARROW(a.edge, Edge) DO 
          e.style := edgeStyle
        END;
        a := Onext(Onext(Sym(a)));
        IF i MOD 2 # 0 THEN a := Onext(Onext(a)) END;
      END;
    END SetQuarterEdge;
    
  PROCEDURE SetLeftTriangle(t: Face) =
    BEGIN
      t.patch := facePatch;
      t.style := faceStyle;
    END SetLeftTriangle;
  
  PROCEDURE SetRowPatches(d: Arc; row: CARDINAL) =
    BEGIN
      d := Sym(d);
      SetLeftTriangle(Left(d));
      d := Onext(d);
      FOR col := 1 TO order-row-1 DO 
        d := Onext(d);
        SetLeftTriangle(Left(d));
        d := Sym(d);
        SetLeftTriangle(Left(d));
        d := Onext(Flip(d));
      END
    END SetRowPatches;
    
  BEGIN
    SetCornerVertex(Org(c));
    SetQuarterEdge(Oprev(c));
    FOR row := 0 TO order-1 DO 
      SetRowPatches(c, row);
      c := Oprev(Sym(c))
    END
  END SetQuarterGridStyles;
 
PROCEDURE Glue(a, b: Arc; n: CARDINAL): Arc =
  VAR ta, tb, pa: Arc;
  BEGIN
    (* Sanity check: *)
    <* ASSERT n >= 1 *>
    ta := a; tb := b;
    FOR i := 2 TO n DO 
      ta := Lnext(ta); tb := Rprev(tb);
      <* ASSERT ta # a *>
      <* ASSERT tb # b *>
    END;
    
    Splice(a, Oprev(b));
    SetAllOrgs(a, Org(a));
    
    FOR i := 1 TO n DO
      ta := Lnext(a); tb := Rprev(b);
      Splice(ta, Oprev(tb));
      <* ASSERT Onext(a) = b *>
      <* ASSERT Onext(Sym(b)) = Sym(a) *>
      SetAllOrgs(ta, Org(ta));
      (* Disconnect b: *)
      WITH f = Left(b) DO
        Splice(b, Oprev(b));
        Splice(Sym(b), Oprev(Sym(b)));
        SetLeft(a, f)
      END;
      pa := a; 
      a := ta; b := tb
    END;
    
    RETURN pa
  END Glue;

(* === GLOBAL PROCEDURES === *)

PROCEDURE NumberVertices(a: Arc): CARDINAL = 
  VAR n: CARDINAL := 0;
  
  PROCEDURE Visit(c: Arc) =
    VAR cn: Arc;  
    BEGIN
      WITH v = Org(c) DO
        cn := c;
        REPEAT
          WITH vn = Org(cn) DO
            <* ASSERT vn = v *>
            vn.num := n
          END;
          cn := Onext(cn);
        UNTIL (cn = c);
      END;
      INC(n);
    END Visit;
    
  BEGIN
    Oct.EnumVertices(a, Visit);
    RETURN n
  END NumberVertices;

PROCEDURE MakeTopology(a: Arc): Topology =
  VAR top: Topology;
  
  BEGIN
    top.NV := NumberVertices(a); 
    top.NF := NumberVertices(Rot(a));

    top.edge := Oct.NumberEdges(ARRAY OF Arc{a});
    top.NE := NUMBER(top.edge^);
    
    top.out := NEW(REF ARRAY OF Arc, top.NV); 
    top.vertex := NEW(REF ARRAY OF Vertex, top.NV);
    top.face := NEW(REF ARRAY OF Face, top.NF);
    top.side := NEW(REF ARRAY OF Arc, top.NF); 

    FOR ei := 0 TO top.NE-1 DO
      VAR c: Arc := top.edge[ei];
      BEGIN
        FOR k := 0 TO 1 DO
          WITH
            v = Org(c), vi = v.num,
            f = Left(c), fi = f.num 
          DO
            top.vertex[vi] := v; top.out[vi] := c;
            top.face[fi] := f; top.side[fi] := Tor(c);
          END;
          c := Sym(c)
        END
      END
    END;
    RETURN top
  END MakeTopology;

PROCEDURE MakeAdjacencyMatrix(READONLY top: Topology): REF AdjacencyMatrix =
  VAR m := NEW(REF ARRAY OF ARRAY OF BOOLEAN, top.NV, top.NV);
  BEGIN
    FOR i := 0 TO top.NV-1 DO 
      WITH mi = m[i] DO 
        FOR j := 0 TO top.NV-1 DO 
          mi[j] := FALSE
        END
      END
    END;
    FOR ei := 0 TO top.NE-1 DO
      WITH a = top.edge[ei] DO
        WITH i = Org(a).num, j = Org(Sym(a)).num DO
          m[i,j] := TRUE;
          m[j,i] := TRUE;
        END
      END
    END;
    RETURN m
  END MakeAdjacencyMatrix;

PROCEDURE CheckOutAndSide(READONLY top: Topology) =
  BEGIN
    FOR i := 0 TO top.NV-1 DO 
      WITH
        v = top.vertex[i],
        e = top.out[i]
      DO
        <* ASSERT v.num = i *>
        <* ASSERT Org(e) = v *>
      END
    END;
    FOR i := 0 TO top.NF-1 DO
      WITH
        f = top.face[i],
        e = top.side[i]
      DO
        <* ASSERT f.num = i *>
        <* ASSERT Org(e) = f *>
      END
    END
  END CheckOutAndSide;
  
PROCEDURE GetVariableVertices(READONLY top: Topology; VAR vr: ARRAY OF BOOLEAN) =
  BEGIN
    FOR i := 0 TO top.NV-1 DO
      WITH v = top.vertex[i] DO 
        vr[i] := v.exists AND (NOT v.fixed) 
      END;
    END;
  END GetVariableVertices;

PROCEDURE MaxVertexStyle(READONLY v: ARRAY OF Vertex): VertexStyle =
  VAR s: VertexStyle := 0;
  BEGIN
    FOR i := 0 TO LAST(v) DO s := MAX(s, v[i].style) END;
    RETURN s
  END MaxVertexStyle;

PROCEDURE MaxEdgeStyle(READONLY e: ARRAY OF Edge): EdgeStyle =
  VAR s: EdgeStyle := 0;
  BEGIN
    FOR i := 0 TO LAST(e) DO s := MAX(s, e[i].style) END;
    RETURN s
  END MaxEdgeStyle;

PROCEDURE MaxFaceStyle(READONLY f: ARRAY OF Face): FaceStyle =
  VAR s: FaceStyle := 0;
  BEGIN
    FOR i := 0 TO LAST(f) DO s := MAX(s, f[i].style) END;
    RETURN s
  END MaxFaceStyle;

PROCEDURE MaxPatchNum(READONLY f: ARRAY OF Face): PatchNum =
  VAR p: PatchNum := 0;
  BEGIN
    FOR i := 0 TO LAST(f) DO p := MAX(p, f[i].patch) END;
    RETURN p
  END MaxPatchNum;

(* === GEOMETRIC TOOLS === *)

PROCEDURE InitCoords(
    coins: Random.T; 
    VAR c: Coords; 
    radius: REAL := 1.0
  ) =
  BEGIN
    WITH r = FLOAT(radius, LONGREAL) DO
      FOR i := 0 TO LAST(c) DO
        c[i] := LR3.T{
          coins.longreal(-r, r),
          coins.longreal(-r, r),
          coins.longreal(-r, r) 
        }
      END
    END
  END InitCoords;

PROCEDURE Barycenter(READONLY top: Topology; READONLY c: Coords): LR3.T =
  VAR B: LR3.T := LR3.T{0.0d0, ..};
      N: CARDINAL := 0;
  BEGIN
    FOR i := 0 TO LAST(c) DO 
      WITH v = top.vertex[i] DO
        IF v.exists THEN B := LR3.Add(B, c[i]); INC(N) END;
      END;
    END;
    RETURN LR3.Scale(1.0d0/FLOAT(N, LONGREAL), B)
  END Barycenter;

PROCEDURE MeanVertexNorm(READONLY top: Topology; READONLY c: Coords): LONGREAL =
  VAR S: LONGREAL := 0.0d0;
      N: CARDINAL := 0;
  BEGIN
    FOR i := 0 TO LAST(c) DO 
      WITH v = top.vertex[i] DO
        IF v.exists THEN 
          S := S + LR3.NormSqr(c[i]);
          INC(N)
        END;
      END;
    END;
    RETURN Math.sqrt(S/FLOAT(N,LONGREAL))
  END MeanVertexNorm;

PROCEDURE MeanEdgeLength(READONLY top: Topology; READONLY c: Coords): LONGREAL =
  VAR S: LONGREAL := 0.0d0;
      N: CARDINAL := 0;
  BEGIN
    FOR i := 0 TO top.NE-1 DO 
      WITH a = top.edge[i], e = NARROW(a.edge, Edge) DO
        IF e.exists THEN 
          WITH o = OrgV(a).num, d = OrgV(Sym(a)).num DO 
            S := S + LR3.DistSqr(c[o], c[d])
          END;
          INC(N)
        END;
      END;
    END;
    RETURN Math.sqrt(S/FLOAT(N,LONGREAL))
  END MeanEdgeLength;

PROCEDURE Displace(READONLY top: Topology; d: LR3.T; VAR c: Coords) =
  BEGIN
    FOR i := 0 TO LAST(c) DO 
      IF top.vertex[i].exists THEN
        WITH vc = c[i] DO vc := LR3.Add(vc, d) END
      END
    END
  END Displace;

PROCEDURE Scale(READONLY top: Topology; s: LONGREAL; VAR c: Coords) =
  BEGIN
    FOR i := 0 TO LAST(c) DO 
      IF top.vertex[i].exists THEN
        WITH vc = c[i] DO vc := LR3.Scale(s, vc) END;
      END
    END
  END Scale;

PROCEDURE NormalizeVertexNorms(READONLY top: Topology; VAR c: Coords) =
  BEGIN
    WITH b = Barycenter(top, c) DO Displace(top, LR3.Neg(b), c) END;
    WITH s = MeanVertexNorm(top, c) DO Scale(top, 1.0d0/s, c) END;
  END NormalizeVertexNorms;

PROCEDURE NormalizeEdgeLengths(READONLY top: Topology; VAR c: Coords) =
  BEGIN
    WITH b = Barycenter(top, c) DO Displace(top, LR3.Neg(b), c) END;
    WITH s = MeanEdgeLength(top, c) DO Scale(top, 1.0d0/s, c) END;
  END NormalizeEdgeLengths;

PROCEDURE FaceCross(a: Arc; READONLY c: Coords): LR3.T =
  VAR be: Arc; bo, n: LR3.T; bn: CARDINAL; 
  BEGIN
    IF NOT LeftF(a).exists THEN
      RETURN LR3.T{1.0d0, 0.0d0, 0.0d0}
    ELSE
      WITH
        on = OrgV(a).num,
        o = c[on],
        an = OrgV(Sym(a)).num 
      DO
        be := a;
        bn := an;
        bo := LR3.Sub(c[bn], o);
        LOOP
          WITH
            ce = Lnext(be),
            cn = OrgV(Sym(be)).num
          DO
            IF cn = on THEN EXIT END;
            WITH 
              co = LR3.Sub(c[cn], o),
              r = LR3Extras.Cross(bo, co)
            DO
              IF bn = an THEN n := r ELSE n := LR3.Add(n, r) END;
              bn := cn; bo := co; be := ce
            END
          END
        END
      END;
      RETURN n
    END
  END FaceCross;

PROCEDURE FaceNormal(a: Arc; READONLY c: Coords): LR3.T =
  BEGIN
    WITH
      n = FaceCross(a, c),
      s = LR3.Norm(n)
    DO
      IF s = 0.0d0 THEN 
        RETURN LR3.T{1.0d0, 0.0d0, 0.0d0}
      ELSE
        RETURN LR3.Scale(1.0d0/s, n)
      END
    END
  END FaceNormal;

PROCEDURE FaceBarycenter(a: Arc; READONLY c: Coords): LR3.T =
  BEGIN
    WITH
      e = Lnext(a),
      i = Lnext(e),
      ac = c[OrgV(a).num],
      ec = c[OrgV(e).num],
      ic = c[OrgV(i).num],
      bar = LR3.Scale(1.0d0/3.0d0, LR3.Add(LR3.Add(ac, ec), ic))
    DO
      RETURN bar
    END
  END FaceBarycenter;

PROCEDURE VertexCross(a: Arc; READONLY c: Coords): LR3.T =
  VAR ao: Arc; 
      sum: LR3.T := LR3.T{0.0d0, ..};
  BEGIN
    WITH
      uv = OrgV(a),
      u = c[uv.num]
    DO
      IF NOT uv.exists THEN
        RETURN LR3.T{1.0d0, 0.0d0, 0.0d0}
      ELSE
        ao := a;
        REPEAT
          WITH
            an = Onext(ao),
            ov = OrgV(Sym(ao)), (* DestV(ao) *)  
            nv = OrgV(Sym(an))  (* DestV(an) *)
          DO
            IF ov.exists AND nv.exists THEN
              WITH 
                o = c[ov.num],
                n = c[nv.num],
                r = LR3Extras.Cross(LR3.Sub(o, u), LR3.Sub(n, u))
              DO
                IF ao = a THEN sum := r ELSE sum := LR3.Add(sum, r) END
              END
            END;
            ao := an
          END;
        UNTIL (ao = a);
        RETURN sum
      END
    END
  END VertexCross;

PROCEDURE VertexNormal(a: Arc; READONLY c: Coords): LR3.T =
  BEGIN
    WITH
      n = VertexCross(a, c),
      s = LR3.Norm(n)
    DO
     IF s = 0.0d0 THEN 
        RETURN LR3.T{1.0d0, 0.0d0, 0.0d0}
      ELSE
        RETURN LR3.Scale(1.0d0/s, n)
      END
    END
  END VertexNormal;

PROCEDURE NeighborBarycenter(a: Arc; READONLY c: Coords): LR3.T =
  VAR an: Arc;
      n: CARDINAL := 0;
      b := LR3.T{0.0d0, ..};
  BEGIN
    an := a;
    REPEAT
      WITH v = OrgV(Sym(an)) DO
        IF v.exists THEN b := LR3.Add(b, c[v.num]); INC(n) END
      END;
      an := Onext(an)
    UNTIL (an = a);
    IF n = 0 THEN
      RETURN b
    ELSE
      RETURN LR3.Scale(1.0d0/FLOAT(n, LONGREAL), b)
    END;
  END NeighborBarycenter;

PROCEDURE ComputeAllVertexNormals(
    READONLY top: Topology; 
    READONLY c: Coords;
  ): REF ARRAY OF LR3.T =
  BEGIN
    WITH
      rvn = NEW(REF ARRAY OF LR3.T, top.NV),
      vn = rvn^
    DO
      FOR i := 0 TO top.NV-1 DO 
        vn[i] := VertexNormal(top.out[i], c)
      END;
      RETURN rvn
    END;
  END ComputeAllVertexNormals; 

BEGIN
END Triang.