MODULE SPFixTriang EXPORTS Main;

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

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

TYPE 
  Point = SPTriang.Coords;
  Points = ARRAY OF Point;
  LONG = LONGREAL;
  NAT = CARDINAL;
 
TYPE 
  Options = RECORD
      inName: TEXT;         (* Input triang file (minus ".tri" extension) *)
      minAngle: LONGREAL;   (* Minimum allowed angle between edge planes. *)
      maxDisp: LONGREAL;    (* Maximum vertex displacement allowed. *)
      maxIter: CARDINAL;    (* Maximum vertex adjustments allowed. *)
      maxTrials: CARDINAL;  (* Maximum guesses allowed when fixing one vertex. *)
      outName: TEXT;        (* Output triang file (minus ".tri" extension) *)
    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);
      SPTriang.ComputeTriangleMatrices(tri);
      WITH wr = OpenWrite(o.outName & ".tri") DO
        SPTriang.Write(wr, tri);
      END;
    END;
  END Main;

PROCEDURE AdjustVertices(
    tri: SPTriang.T;
    READONLY o: Options;
  ) =
  VAR iters: NAT;
      aMin: LONG;
      eMin, fMin: Arc;
  BEGIN
    WITH
      NV = NUMBER(tri.out^),
      out = tri.out^,
      vOld = NEW(REF Points, NV)^,
      rnd = NEW(Random.Default).init(fixed := TRUE)
    DO
      FOR i := 0 TO NV-1 DO
        WITH
          vi = SPTriang.Org(out[i])
        DO
          <* ASSERT vi.num = i *>
          vOld[i] := vi.c
        END
      END;
      iters := 0;
      LOOP
        FindWorstVertex(out, (*OUT*) aMin, eMin, fMin);
        Wr.PutText(stderr, "aMin = " & FLR(aMin,6,4));
        Wr.PutText(stderr, " iMin = " & Fmt.Int(Org(eMin).num) & "\n");
        IF aMin >= o.minAngle THEN
          Wr.PutText(stderr, "\n");
          Wr.PutText(stderr, "succeeded in " & Fmt.Int(iters) & " iterations\n");
          RETURN
        END;
        IF iters >= o.maxIter THEN 
          Wr.PutText(stderr, "failed after " & Fmt.Int(iters) & " iterations\n");
          RETURN
        END;
        PerturbVertex(
          eMin, fMin, 
          vOld[Org(eMin).num], 
          rnd,
          minAngle := o.minAngle, 
          maxDisp := o.maxDisp, 
          maxTrials := o.maxTrials
        );
        INC(iters)
      END;
    END
  END AdjustVertices;
  
PROCEDURE FindWorstVertex(
    READONLY out: ARRAY OF Arc; (* One arc out of each vertex *)
    VAR aMin: LONG;             (* OUT: the smallest suplem. edge-edge angle *)
    VAR eMin, fMin: Arc;        (* OUT: the pair of edges which realize "aMin". *)
  ) =
  VAR a, b, c: Arc;
  BEGIN
    aMin := LAST(LONG);
    FOR i := 0 TO LAST(out) DO
      WITH
        start = out[i]
      DO
        a := start; b := a;
        REPEAT
          (* Finds arc "b" roughly opposite to "a". *)
          (* More precisely, such that the direction opposite to "a" *)
          (* lies between the direction of "b" (inclusive) and *)
          (* that of "Onext(b)" (exclusive). *)
          (* Assumes that all corners are less than 180 degrees. *)
          <* ASSERT b = a OR AngleSign(a, b) = +1 *>
          c := Onext(b);
          WHILE b = a OR AngleSign(a, c) >= 0 DO b := c; c := Onext(b) END;
          <* ASSERT c # a *>
          WITH ang = ABS(SupplAngle(a, b)) DO 
            IF ang < aMin THEN aMin := ang; eMin := a; fMin := b; END;
          END;
          WITH ang = ABS(SupplAngle(a, c)) DO 
            IF ang < aMin THEN aMin := ang; eMin := a; fMin := c; END;
          END;
          a := Onext(a);
        UNTIL a = start;
      END;
    END;
  END FindWorstVertex;
  
PROCEDURE SupplAngle(READONLY a, b: Arc): LONG =
  (*
    Returns the angle between the direction of "b" and the direction
    opposite to the direction of "a" (in radians, between "0" and
    "Pi").  That is, returns the angle between the planes containing
    the two arcs.
  *)
  BEGIN
    WITH
      pa = Dest(a).c,
      pb = Dest(b).c,
      po = Org(a).c,
      na = LR3.Dir(LR3Extras.Cross(po, pa)),
      nb = LR3.Dir(LR3Extras.Cross(pb, po)),
      s = LR3.Norm(LR3Extras.Cross(na, nb))
    DO
      RETURN Math.asin(s)
    END
  END SupplAngle;
  
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 PerturbVertex(
    READONLY a, b: Arc;    (* Offending edges. *)
    READONLY pOld: Point;  (* Original position of "Org(a)". *)
    rnd: Random.T;         (* Random number generator. *)
    minAngle: LONG;        (* Minimum angle allowed. *)
    maxDisp: LONG;         (* Maximum perturbation allowed (radians). *)
    maxTrials: NAT;        (* Maximum attempts. *)
  ) =
  (*
    Tries to perturb the vertex "v = Org(a) = Org(b)" so that 
    the angle between the planes of the two edges becomes
    "minAngle" or more, but without moving the vertex by more 
    than "maxDisp" away from its original position. 
  *)
  VAR pBest: Point;  (* Best position without negative triangles *)
      aBest: LONG;   (* The angle observed at that position. *)
      trials: NAT := 0;
  BEGIN
    <* ASSERT Org(a) = Org(b) *>
    WITH
      vo = Org(a)
    DO
      pBest := vo.c;
      Wr.PutText(stderr, "  initial (" & FLR3(vo.c, 8,5) & ")\n");
      aBest := ABS(SupplAngle(a, b));
      WHILE aBest < minAngle AND trials < maxTrials DO
        vo.c := RandomPoint(rnd, pOld, maxDisp);
        Wr.PutText(stderr, "  trying  (" & FLR3(vo.c, 8,5) & ")");
        IF AllPositive(a) THEN
          Wr.PutText(stderr, " ok");
          WITH ang = ABS(SupplAngle(a,b)) DO
            IF ang > aBest THEN 
              aBest := ang; pBest := vo.c; Wr.PutText(stderr, " better\n");
            ELSE
              Wr.PutText(stderr, " no better\n");
            END
          END
        ELSE
          Wr.PutText(stderr, " bad\n");
        END;
        INC(trials)
      END;
      vo.c := pBest;
    END;
  END PerturbVertex;    
  
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 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("-minAngle") THEN
          o.minAngle := pp.getNextLongReal(0.0d0, 1.0d0)
        ELSE
          o.minAngle := 0.05d0
        END;
      
        IF pp.keywordPresent("-maxDisp") THEN
          o.maxDisp := pp.getNextLongReal(0.0d0, 1.0d0)
        ELSE
          o.maxDisp := 0.05d0
        END;
        
        IF pp.keywordPresent("-maxIter") THEN
          o.maxIter := pp.getNextInt(0, LAST(CARDINAL));
        ELSE
          o.maxIter := LAST(CARDINAL)
        END;
        
        IF pp.keywordPresent("-maxTrials") THEN
          o.maxTrials := pp.getNextInt(0, LAST(CARDINAL));
        ELSE
          o.maxTrials := 5
        END;
        
        pp.finish();
      END
    EXCEPT                                                            
    | ParseParams.Error =>                                              
        Wr.PutText(stderr, "Usage: SPDrawTriang \\\n");
        Wr.PutText(stderr, "  -inName <name>  \\\n");
        Wr.PutText(stderr, "  [ -minAngle   NUM ] \\\n");
        Wr.PutText(stderr, "  [ -maxDisp   NUM ] \\\n");
        Wr.PutText(stderr, "  [ -maxIter NUM ]  \\\n");
        Wr.PutText(stderr, "  [ -maxTrials NUM ]  \\\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 SPFixTriang.