MODULE SPGradTest EXPORTS Main;

(* Checks whether the "grad" method is consistent with the "eval" method. *)

IMPORT Wr, Rd, Fmt, FileRd, Text, Thread, OSError, Process, ParseParams;
IMPORT SPPWFunction, SPFunction, SPPlot, SPTriang, LR3, LR4, HLR3, Random; 
FROM SPPWFunction IMPORT Point;
FROM Stdio IMPORT stderr, stdin;
FROM Math IMPORT sin, cos, sqrt;

TYPE 
  LONG = LONGREAL;
  
  Options = RECORD
      triName: TEXT;         (* Triang file (minus ".tri" extension) or "-" *)
      sfnName: TEXT;         (* Function file (minus ".sfn" extension) or "-" *)
      outName: TEXT;         (* Prefix for output file names. *)
      gridSize: CARDINAL;    (* Number of grid steps in Lat & Lon *)
      delta: LONG;           (* Step for numerical gradient computation. *)
      (* Plotting options: *)
      obs: HLR3.Point;       (* Position of observer *)
      eMax: LONGREAL;        (* Nominal max absolute approx error. *)
      eStep: REAL;           (* Value difference between successive error level curves *)
    END;
    
CONST
  Pi = 3.1415926535897932384626433833d0;

<* FATAL Wr.Failure, Thread.Alerted, OSError.E *>

PROCEDURE Main() =
  BEGIN
    WITH 
      o = GetOptions(),
      triRd = FileRd.Open(o.triName & ".tri"),
      tri = MakeTriRef(SPTriang.Read(triRd)),
      sfnRd = OpenSFNReader(o.sfnName),
      fn = NEW(SPPWFunction.T),
      rnd = NEW(Random.Default).init(fixed := TRUE)
    DO  
      fn.tri := tri;
      fn.read(sfnRd);
      CheckGradError(fn, o, rnd);
      PlotGradError(fn, tri, o, rnd);
    END
  END Main;

PROCEDURE CheckGradError(
    fn: SPFunction.T;
    READONLY o: Options;
    rnd: Random.T;
  ) =
  VAR toterr2: LONG := 0.0d0; (* Sum of error squared in gradient *)
      totcos2: LONG := 0.0d0; (* Sum of cos(p,grad)^2 *)
      totpt: CARDINAL := 0;   (* Number of sample points *)
      maxerr: LONG := 0.0d0;  (* Largest error in gradient *)
      maxcos: LONG := 0.0d0;  (* Largest cos(p, grad) *)
      badp, baddp, badq: LR3.T;  (* Points of maximum error *)
      badfp, badfq: LONG;        (* Function values at maximum error points *)
      badg: LR3.T;               (* Gradient at maximum error point *)
  BEGIN
    WITH N = o.gridSize DO
      FOR i := - (N-1) TO (N-1) DO 
        FOR j := 0 TO 2*N-1 DO
          WITH
            lat = Pi/2.0d0 * (FLOAT(i,LONG)/FLOAT(o.gridSize,LONG)),
            lng = Pi * (FLOAT(j,LONG)/FLOAT(o.gridSize,LONG)),
            x = cos(lat)*cos(lng),
            y = cos(lat)*sin(lng),
            z = sin(lat),
            p = LR3.T{x, y, z},
            q = RandomNeighbor(p, o.delta, rnd),
            dp = LR3.Sub(q, p),
            fp = fn.eval(p),
            fq = fn.eval(q),
            g = fn.grad(p),
            gdp = LR3.Dot(g, dp),
            error = (fq - fp) - gdp,
            cosgp = LR3.Dot(g, p)/(LR3.Norm(g) + 1.0d-100)
          DO
            toterr2 := toterr2 + error*error;
            IF ABS(error) > ABS(maxerr) THEN
              maxerr := error;
              badp := p; badfp := fp;
              badq := q; badfq := fq;
              baddp := dp; badg := g
            END;
            totcos2 := totcos2 + cosgp*cosgp;
            IF ABS(cosgp) > ABS(maxcos) THEN maxcos := cosgp END;
            totpt := totpt + 1
          END
        END
      END;
      WITH
        avgerr = sqrt(toterr2/FLOAT(totpt,LONG)),
        avgcos = sqrt(totcos2/FLOAT(totpt,LONG))
      DO
        Wr.PutText(stderr, "avg gradient error = " & FLR(avgerr, 9,6) & "\n");
        Wr.PutText(stderr, "max gradient error = " & FLR(maxerr, 9,6) & "\n");
        Wr.PutText(stderr, "  p =    " & FLR3(badp) & " f(p) = " & FLR(badfp,9,6) & "\n");
        Wr.PutText(stderr, "  q =    " & FLR3(badq) & " f(q) = " & FLR(badfq,9,6) & "\n");
        Wr.PutText(stderr, "  dp =   " & FLR3(baddp) & "\n");
        Wr.PutText(stderr, "  grad = " & FLR3(badg) & "\n");
        Wr.PutText(stderr, "\n");
        Wr.PutText(stderr, "avg gradient angle = " & FLR(avgcos, 9,6) & "\n");
        Wr.PutText(stderr, "max gradient angle = " & FLR(maxcos, 9,6) & "\n");
      END
    END
  END CheckGradError;

PROCEDURE PlotGradError(
    fn: SPFunction.T;
    tri: REF SPTriang.T;
    READONLY o: Options;
    rnd: Random.T;
  ) =
  
  PROCEDURE Err(READONLY p: Point): LONGREAL =
    BEGIN
      RETURN GradError(fn, p, o.delta, rnd)
    END Err;

  VAR obs: HLR3.Point;
      tag: TEXT;

  BEGIN
    IF LR4.Norm(o.obs.c) = 0.0d0 THEN
      (* Pick an arbitrary observer *)
      obs := HLR3.Point{c := LR4.T{0.0d0, 4.0d0, 3.0d0, 2.0d0}};
      Wr.PutText(stderr, "using obs = " & FHLR3(obs) & "\n");
    ELSE
      obs := o.obs;
    END;
    
    FOR side := -1 TO +1 BY 2 DO
      IF side = +1 THEN tag := "v" ELSE tag :=  "f" END;
      SPPlot.Everything(
        func := Err,
        tri := tri,
        outName := o.outName & "-gerr-" & tag,
        fMax := o.eMax,
        fStep := o.eStep,
        M := o.gridSize, N := o.gridSize,
        obs := obs,
        upp := HLR3.Point{c := LR4.T{0.0d0, 0.0d0, 0.0d0, 1.0d0}},
        dLight := LR3.T{0.0d0, 0.0d0, 1.0d0},
        eps := TRUE,
        verbose := TRUE
      );
      (* Reverse position of observer: *)
      FOR k := 1 TO 3 DO obs.c[k] := - obs.c[k] END;
    END
  END PlotGradError;
  
PROCEDURE GradError(
    fn: SPFunction.T;
    READONLY p: Point;
    delta: LONG;
    rnd: Random.T;
  ): LONG =
  BEGIN
    WITH
      q = RandomNeighbor(p, delta, rnd),
      dp = LR3.Sub(q, p),
      fp = fn.eval(p),
      fq = fn.eval(q),
      g = fn.grad(p),
      gdp = LR3.Dot(g, dp),
      error = (fq - fp) - gdp
    DO
      RETURN error
    END
  END GradError;

PROCEDURE RandomNeighbor(READONLY p: Point; delta: LONG; rnd: Random.T): Point =
  BEGIN
    WITH
      rv = LR3.URandom(rnd, -1.0d0, +1.0d0),
      dp = LR3.Scale(delta, LR3.Dir(LR3.Orthize(rv, p))),
      q = LR3.Dir(LR3.Add(p, dp))
    DO
      RETURN q
    END
  END RandomNeighbor;

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 MakeTriRef(READONLY tri: SPTriang.T): REF SPTriang.T = 
  BEGIN
    WITH t = NEW(REF SPTriang.T) DO t^ := tri; RETURN t END
  END MakeTriRef;
  
PROCEDURE OpenSFNReader(name: TEXT): Rd.T =
  BEGIN
    IF Text.Equal(name, "-") THEN 
      Wr.PutText(stderr, "reading function from stdin\n");
      RETURN stdin
    ELSE
      Wr.PutText(stderr, "reading function from " & name & ".sfn\n");
      RETURN FileRd.Open(name & ".sfn")
    END
  END OpenSFNReader;

PROCEDURE GetOptions (): Options =
  VAR o: Options;
  (*  obs, upp: HLR3.Point;*)
  BEGIN
    TRY
      WITH  pp = NEW(ParseParams.T).init(stderr) DO 
          
        pp.getKeyword("-triName");                           
        o.triName := pp.getNext();  
      
        IF pp.keywordPresent("-sfnName") THEN                              
          o.sfnName := pp.getNext()
        ELSE
          o.sfnName := "-"
        END;  
                  
        IF pp.keywordPresent("-gridSize") THEN
          o.gridSize := pp.getNextInt(5, 1000)
        ELSE
          o.gridSize := 100
        END;
      
        pp.getKeyword("-outName");                               
        o.outName := pp.getNext();  
       
        pp.getKeyword("-delta");
        o.delta := pp.getNextLongReal(0.0d0, 1.0d0);
      
        (* Plotting options: *)
        
        IF pp.keywordPresent("-obs") THEN
          o.obs := ParsePointOption(pp)
        ELSE
          (* Default is all zeros, meaning rotate sphere for best view. *)
          o.obs := HLR3.Point{c := LR4.T{0.0d0, 0.0d0, 0.0d0, 0.0d0}}
        END;

        IF pp.keywordPresent("-eMax") THEN
          o.eMax := pp.getNextLongReal(0.0d0, LAST(LONGREAL))
        ELSE
          o.eMax := +1.0d0
        END;
        
        IF pp.keywordPresent("-eStep") THEN
          o.eStep := pp.getNextReal()
        ELSE
          o.eStep := 0.1
        END;
        
        pp.finish();
      END
    EXCEPT                                                            
    | ParseParams.Error =>                                              
        Wr.PutText(stderr, "Usage: SPGradTest \\\n");
        Wr.PutText(stderr, "  -triName NAME\\\n");
        Wr.PutText(stderr, "  [ -sfnName NAME ]\\\n");
        Wr.PutText(stderr, "  [ -gridSize NUM ]\\\n");
        Wr.PutText(stderr, "  -delta NUM \\\n");
        Wr.PutText(stderr, "  [ -obs OW OX OY OZ ] \\\n");
        Wr.PutText(stderr, "  [ -eMax NUM ] [ -eStep NUM ]\n");
        Process.Exit (1);
    END;
    RETURN o
  END GetOptions;
  
PROCEDURE ParsePointOption(pp: ParseParams.T): HLR3.Point RAISES {ParseParams.Error} =
  VAR p: HLR3.Point;
  BEGIN
    FOR i := 0 TO 3 DO
      p.c[i] := pp.getNextLongReal(-1.0d20, +1.0d20)
    END;
    RETURN p
  END ParsePointOption;  

PROCEDURE FLR3(READONLY p: LR3.T): TEXT =
  BEGIN
    RETURN "(" & FLR(p[0],7,4) & ", " & FLR(p[1],7,4) & ", " & FLR(p[2],7,4) & ")" 
  END FLR3;  
  
PROCEDURE FHLR3(p: HLR3.Point): TEXT =
  BEGIN
    RETURN
      "{ " &
      Fmt.LongReal(p.c[0], Fmt.Style.Fix, 2) & 
      ", " &
      Fmt.LongReal(p.c[1], Fmt.Style.Fix, 2) & 
      ", " &
      Fmt.LongReal(p.c[2], Fmt.Style.Fix, 2) & 
      ", " &
      Fmt.LongReal(p.c[3], Fmt.Style.Fix, 2) & 
      " }"
  END FHLR3;

BEGIN
  Main();
END SPGradTest.