MODULE SPOptTriang EXPORTS Main;

(* Optimizes the geometry of a triangulation on the sphere *)

IMPORT Wr, Fmt, Random, Thread, Process, ParseParams;
IMPORT  SPTriang, LR3Extras, LR3, Minimizer, CoordMinimizer, BrentUniMin;
FROM SPTriang IMPORT Dest, Org;
FROM SPQuad IMPORT Arc, Onext, Lnext;
FROM SPFile IMPORT OpenWrite, OpenRead;
FROM Stdio IMPORT stderr;

TYPE 
  Point = SPTriang.Coords;
  LONG = LONGREAL;
  NAT = CARDINAL;
 
TYPE 
  Options = RECORD
      inName: TEXT;          (* Input triang file (minus ".tri" extension) *)
      outName: TEXT;         (* Output triang file (minus ".tri" extension) *)
      (* Parameters for triangulation ugliness function: *)
      colWeight: LONGREAL;   (* Weight for edge collinearity term. *)
      shapeWeight: LONGREAL; (* Weight for triangle shape term. *)
      sizeWeight: LONGREAL;  (* Weight for triangle size term. *)
      (* Global optimization parameters: *)
      maxTrials: CARDINAL;  (* Maximum restartings. *)
      (* Parameters for local ugliness optimization: *)
      maxEvals: CARDINAL;   (* Maximum ugliness evaluations per trial. *)
      tol: LONG;            (* Est. diameter of the minimum region. *)
      (* Parameters for restarting the optimization: *)
      maxTosses: NAT;       (* Maximum random points generated per vertex. *)
      tossRadius: LONGREAL;  (* Average vertex displacement when restarting. *)
    END;

<* FATAL Wr.Failure, Thread.Alerted *>
  
PROCEDURE Main() =
  VAR tri: SPTriang.T;
  BEGIN
    WITH 
      o = GetOptions(),
      rd = OpenRead(o.inName & ".tri")
    DO
      tri := SPTriang.Read(rd);
      AdjustVertices(tri, o);
    END;
  END Main;

PROCEDURE SetVertexCoords(tri: SPTriang.T; READONLY x: Minimizer.Point) =
  (* 
    Unpacks the vertex coordinates from the vector "x" and stores them
    in the triangulation "tri" itself.
  *)
  BEGIN 
    WITH
      out = tri.out^,
      NV = NUMBER(out)
    DO
      FOR i := 0 TO NV-1 DO
        WITH
          k = 3*i,
          vc = Org(out[i]).c
        DO
          vc[0] := x[k];
          vc[1] := x[k+1];
          vc[2] := x[k+2];
          vc := LR3.Dir(vc);
        END
      END
    END
  END SetVertexCoords;

PROCEDURE GetVertexCoords(tri: SPTriang.T; VAR  x: Minimizer.Point) =
  (* 
    Extracts the vertex coordinates from the triangulation "tri"
    and packs them into the vector "x".
  *)
  BEGIN 
    WITH
      out = tri.out^,
      NV = NUMBER(out)
    DO
      FOR i := 0 TO NV-1 DO
        WITH
          k = 3*i,
          vc = Org(out[i]).c
        DO
          x[k]   := vc[0]; 
          x[k+1] := vc[1]; 
          x[k+2] := vc[2]; 
        END
      END
    END
  END GetVertexCoords;


PROCEDURE AdjustVertices(
    tri: SPTriang.T;
    READONLY o: Options;
  ) =
  VAR nTrials: NAT;
      f: LONG;
      fBest: LONG := LAST(LONG);
  BEGIN
    WITH
      NV = NUMBER(tri.out^),
      xBest = NEW(REF Minimizer.Point, 3*NV)^,
      rnd = NEW(Random.Default).init(fixed := TRUE)
    DO
      nTrials := 0;
      WHILE nTrials < o.maxTrials DO
        OptimizeVertexPositions(tri, 0.1d0, o.maxEvals, o.tol, 
          o.colWeight, o.shapeWeight, o.sizeWeight,
          (*OUT*) f
        );
        INC(nTrials);
        IF f < fBest THEN 
          GetVertexCoords(tri, xBest); fBest := f;
          WriteTriang(tri, o.outName & "-better")        
        END;
        PerturbVertices(tri, rnd, o.tossRadius, o.maxTosses);
      END;
      SetVertexCoords(tri, xBest);
      WriteTriang(tri, o.outName)   
    END
  END AdjustVertices;
  
PROCEDURE WriteTriang(tri: SPTriang.T; name: TEXT) =
  BEGIN
    SPTriang.ComputeTriangleMatrices(tri);
    WITH wr = OpenWrite(name & ".tri") DO
      SPTriang.Write(wr, tri);
    END;
  END WriteTriang;
  
PROCEDURE OptimizeVertexPositions(
    tri: SPTriang.T;       (* Triangulation *)
    dist: LONGREAL;        (* Guessed distance from optimum *)
    maxEvals: NAT;         (* Maximum evaluations *)
    tol: LONGREAL;         (* Guessed radius of optimum region *)
    (* Parameters for ugliness function: *)
    colWeight: LONGREAL;   (* Weight for edge collinearity term. *)
    shapeWeight: LONGREAL; (* Weight for triangle shape term. *)
    sizeWeight: LONGREAL;  (* Weight for triangle size term. *)
    (*OUT*)
    VAR f: LONG;           (* Ugliness of optimized triangulation *)
  ) = 

  VAR nEvals: CARDINAL;
  BEGIN
    WITH
      out = tri.out^,
      NV = NUMBER(out),
      uMin = NEW(BrentUniMin.T),
      cMin = NEW(CoordMinimizer.T).setUniMin(uMin).setBudget(5).setSize(3*NV), 
      g    = NEW(REF Minimizer.Gradient, 3*NV)^,
      x    = NEW(REF Minimizer.Point, 3*NV)^
    DO

      PROCEDURE Evaluate(
         READONLY x: Minimizer.Point; 
         VAR f: Minimizer.Value; 
         <*UNUSED*> VAR g: Minimizer.Gradient
        ): BOOLEAN =
        BEGIN
          IF nEvals >= maxEvals THEN RETURN TRUE END;
          SetVertexCoords(tri, x);
          f := Ugliness(tri, colWeight, shapeWeight, sizeWeight);
          INC(nEvals);
          Wr.PutText(stderr, "  evals = " & Fmt.Int(nEvals));
          Wr.PutText(stderr, "  ugliness f = " & FLR(f,12,4) & "\n");
          RETURN FALSE;
        END Evaluate;
  
      BEGIN 
        GetVertexCoords(tri, x);
        IF Evaluate(x, f, g) THEN RETURN END;

        Wr.PutText(stderr, "begin local optimization, f = " & FLR(f,12,4) & "\n");
        
        cMin.debug := FALSE; 
        cMin.minimize(
          eval := Evaluate, (* Evaluates the goal function. *)
          x := x,           (* In: initial guess, out: minimum point. *) 
          fx := f,          (* In/out: function value at "x" *)
          dfx := g,         (* In/out: gradient of "f" at "x" *)
          dist := dist,     (* Guessed distance from optimum *)
          tol := tol,       (* Guessed radius of optimum region *)
          check := NIL      (* Acceptance test *)    
        );

        EVAL Evaluate(x, f, g);
        SetVertexCoords(tri, x);
        Wr.PutText(stderr, "end local optimization, f = " & FLR(f,12,4) & "\n");
        
      END;
    END;
  END OptimizeVertexPositions;

VAR debug: BOOLEAN := FALSE;

PROCEDURE Ugliness(
    tri: SPTriang.T; 
    (* Parameters for ugliness function: *)
    colWeight: LONGREAL;   (* Weight for edge collinearity term. *)
    shapeWeight: LONGREAL; (* Weight for triangle shape term. *)
    sizeWeight: LONGREAL;  (* Weight for triangle size term. *)
  ): LONG = 
  VAR f: LONG := 0.0d0;
      a, b: Arc;
  BEGIN
    WITH
      out = tri.out^,
      side = tri.side^,
      NV = NUMBER(out),
      NT = NUMBER(side)
    DO
      (* Compute edge collinarity and sphericity term: *)
      FOR i := 0 TO NV-1 DO 
        (* Enumerates all arcs "a" out of vertex i: *)
        WITH 
          start = out[i], 
          po = Org(start).c,
          r = LR3.NormSqr(po)
        DO
          f := f + (1.0d0/r + r);
          a := Onext(start);
          WHILE a # start DO
            (* Enumerates all arcs "b" between "Onext(a)" and "start": *)
            b := a;
            REPEAT 
              b := Onext(b);
              f := f + colWeight * CollinearTerm(Org(a).c, Dest(a).c, Dest(b).c);
            UNTIL b = start;
            a := Onext(a);
          END;
        END;
      END;
      
      (* Compute shape and size triangle ugliness terms: *)
      FOR i := 0 TO NT-1 DO
        WITH
          a = side[i],   pa = Org(a).c,
          b = Lnext(a),  pb = Org(b).c,
          c = Lnext(b),  pc = Org(c).c
        DO
          f := f + shapeWeight*TriShapeTerm(pa,pb,pc)
                 + sizeWeight*TriSizeTerm(pa,pb,pc)
        END
      END
    END;
    RETURN f
  END Ugliness;

PROCEDURE CollinearTerm(READONLY po, pa, pb: Point): LONG =
  BEGIN
    WITH
      na = LR3.Dir(LR3Extras.Cross(po, pa)),
      nb = LR3.Dir(LR3Extras.Cross(pb, po)),
      s = LR3.NormSqr(LR3Extras.Cross(na, nb))
    DO
      RETURN 1.0d0/(1.0d-20 + s)
    END
  END CollinearTerm;

PROCEDURE TriShapeTerm(READONLY pa, pb, pc: Point): LONG =
  BEGIN
    WITH
      d2ab = LR3.DistSqr(pa,pb),
      d2bc = LR3.DistSqr(pb,pc),
      d2ca = LR3.DistSqr(pc,pa),
      d2 = d2ab+d2bc+d2ca,
      A = LR3Extras.Det(pa,pb,pc)
    DO
      IF debug THEN 
        Wr.PutText(stderr, "    shape term A = " & FLR(A, 12,6));
        Wr.PutText(stderr, " d2 = " & FLR(d2, 12,6) & "\n");
      END;
      RETURN d2/(d2*1.0d-20 + MAX(0.0d0, A))*(1.0d0 + MAX(0.0d0, -A))
    END
  END TriShapeTerm;

PROCEDURE TriSizeTerm(
    <*UNUSED*> READONLY pa: Point; 
    <*UNUSED*> READONLY pb: Point;
    <*UNUSED*> READONLY pc: Point; 
  ): LONG =
  BEGIN
    RETURN 0.0d0
  END TriSizeTerm;
  
PROCEDURE PerturbVertices(
    tri: SPTriang.T;
    rnd: Random.T;
    step: LONG;
    maxTosses: NAT;
  ) =
  (*
    Tries to perturb every vertex "v"
    randomly, without reversing any triangle. 
  *)
  VAR nTosses: NAT;
      pOld: LR3.T;
  BEGIN
    WITH
      out = tri.out^,
      NV = NUMBER(out)
    DO
      Wr.PutText(stderr, "begin vetex perturbation\n");
      FOR i:= 0 TO NV-1 DO
        WITH a = out[i], vo = Org(a) DO
          pOld := vo.c;
          nTosses := 0;
          Wr.PutText(stderr, "  begin tossing vertex " & Fmt.Int(vo.num) & "\n");
          Wr.PutText(stderr, "    initial (" & FLR3(vo.c, 8,5) & ")");
          LOOP
            IF nTosses >= maxTosses THEN 
              (* Give up, return vertex to original position: *)
              vo.c := pOld;
              <* ASSERT AllPositive(a) *>
              EXIT
            END;
            vo.c := RandomPoint(rnd, pOld, step);
            Wr.PutText(stderr, "    trying  (" & FLR3(vo.c, 8,5) & ")");
            IF AllPositive(a) THEN
              Wr.PutText(stderr, " ok\n"); EXIT;
            ELSE
              Wr.PutText(stderr, " bad\n");
            END;
            INC(nTosses)
          END;
        END;
        Wr.PutText(stderr, "  end tossing\n");
      END;      
      Wr.PutText(stderr, "end vetex perturbation\n");
    END 
  END PerturbVertices;    
  
PROCEDURE AllPositive(e: Arc): BOOLEAN =
  (*
    Returns TRUE if all triangles incident to "Org(a)" have
    positive orientation.
  *)
  VAR a, b: Arc;
  BEGIN
    a := e; b := Onext(a);
    REPEAT
      IF AngleSign(a, b) <= 0 THEN RETURN FALSE END;
      a := b; b := Onext(a)
    UNTIL a = e;
    RETURN TRUE;
  END AllPositive;

PROCEDURE AngleSign(READONLY a, b: Arc): [-1..+1] =
  (*
    Returns the sign of the shortest angle between the directions of
    "a" and "b", which must have the same origin.  That is, returns
    "+1" if the direction of "b" lies strictly between 0 and 180 degrees
    counterclockwise from that of "a", "-1" if "b" lies strictly between
    0 and 180 degrees clockwise, and 0 if the two directions are coincident
    or opposite.
  *)
  BEGIN
    <* ASSERT Org(a) = Org(b) *>
    WITH
      pa = Dest(a).c,
      pb = Dest(b).c,
      po = Org(a).c,
      det = LR3Extras.Det(po, pa, pb)
    DO
      IF det > 0.0d0 THEN
        RETURN +1
      ELSIF det < 0.0d0 THEN
        RETURN -1
      ELSE
        RETURN 0
      END
    END
  END AngleSign;

PROCEDURE GetOptions (): Options =
  VAR o: Options;
  BEGIN
    TRY
      WITH  pp = NEW(ParseParams.T).init(stderr) DO 
          
        pp.getKeyword("-inName");                           
        o.inName := pp.getNext();  
      
        pp.getKeyword("-outName");                               
        o.outName := pp.getNext();  
        
        IF pp.keywordPresent("-colWeight") THEN
          o.colWeight := pp.getNextLongReal(0.001d0, 1.0d0)
        ELSE
          o.colWeight := 1.0d0
        END;
      
        IF pp.keywordPresent("-shapeWeight") THEN
          o.shapeWeight := pp.getNextLongReal(0.001d0, 1.0d0)
        ELSE
          o.shapeWeight := 1.0d0
        END;
      
        IF pp.keywordPresent("-sizeWeight") THEN
          o.sizeWeight := pp.getNextLongReal(0.001d0, 1.0d0)
        ELSE
          o.sizeWeight := 1.0d0
        END;
      
        IF pp.keywordPresent("-maxTrials") THEN
          o.maxTrials := pp.getNextInt(0, LAST(CARDINAL));
        ELSE
          o.maxTrials := 5
        END;
        
        pp.getKeyword("-maxEvals");
        o.maxEvals := pp.getNextInt(0, LAST(CARDINAL));
        
        IF pp.keywordPresent("-tol") THEN
          o.tol := pp.getNextLongReal(0.0d0, 1.0d0)
        ELSE
          o.tol := 0.01d0
        END;
        
        IF pp.keywordPresent("-tossRadius") THEN
          o.tossRadius := pp.getNextLongReal(0.0d0, 1.0d0)
        ELSE
          o.tossRadius := 0.05d0
        END;
        
        IF pp.keywordPresent("-maxTosses") THEN
          o.maxTosses := pp.getNextInt(0, LAST(CARDINAL));
        ELSE
          o.maxTosses := LAST(CARDINAL)
        END;
        
        pp.finish();
      END
    EXCEPT                                                            
    | ParseParams.Error =>                                              
        Wr.PutText(stderr, "Usage: SPDrawTriang \\\n");
        Wr.PutText(stderr, "  -inName <name>  \\\n");
        Wr.PutText(stderr, "  [ -colWeight NUM ] \\\n");
        Wr.PutText(stderr, "  [ -shapeWeight NUM ] \\\n");
        Wr.PutText(stderr, "  [ -sizeWeight NUM ] \\\n");
        Wr.PutText(stderr, "  [ -maxTrials NUM ]  \\\n");
        Wr.PutText(stderr, "  [ -maxEvals NUM ] [ -tol NUM ] \\\n");
        Wr.PutText(stderr, "  [ -maxTosses NUM ] [ -tossRadius] \\\n");
        Wr.PutText(stderr, "  -outName <name> \n");
        Process.Exit (1);
    END;
    RETURN o
  END GetOptions;

PROCEDURE FLR(x: LONGREAL; wid, prec: CARDINAL): TEXT =
  BEGIN
    RETURN Fmt.Pad(Fmt.LongReal(x, style:= Fmt.Style.Sci, prec:= prec), wid)
  END FLR;
  
PROCEDURE FLR3(READONLY p: LR3.T; wid, prec: CARDINAL): TEXT =
  BEGIN
    RETURN 
      Fmt.Pad(Fmt.LongReal(p[0], style:= Fmt.Style.Sci, prec:= prec), wid) &
      ", " &
      Fmt.Pad(Fmt.LongReal(p[1], style:= Fmt.Style.Sci, prec:= prec), wid) &
      ", " &
      Fmt.Pad(Fmt.LongReal(p[2], style:= Fmt.Style.Sci, prec:= prec), wid)
  END FLR3;
  
PROCEDURE RandomPoint(rnd: Random.T; READONLY p: Point; rad: LONG): Point =
  VAR s: LR3.T;
  BEGIN
    REPEAT
      FOR i := 0 TO 2 DO s[i] := rnd.longreal(-1.0d0, +1.0d0) END
    UNTIL LR3.NormSqr(s) < 1.0d0;
    RETURN LR3.Dir(LR3.Add(p, LR3.Scale(rad, s)))
  END RandomPoint;
  
BEGIN
  Main();
END SPOptTriang.