MODULE SimMin;

IMPORT Math;

IMPORT Wr, Thread, Fmt;
FROM Stdio IMPORT stderr;

PROCEDURE Minimize (
    f: GoalFunc;
    VAR x: ARRAY OF ARRAY OF REAL;   (* In/Out: the probe simplex. *)
    VAR y: ARRAY OF REAL;            (* Out: values of "f" at simplex vertices. *)         
    VAR lo, hi: CARDINAL;            (* Out: lowest and highest vertices. *) 
    minSize: REAL;                   (* Minimum simplex size. *)
    maxCalls: CARDINAL;              (* Maximum number of calls. *)
    report: ReportProc := NIL;       (* Called when a new minimum is found. *)
    normalize: NormalizeProc := NIL; (* Called to modify point before applying "f". *)
    verbose: BOOLEAN := FALSE;       (* TRUE to mumble while working. *)
    Alpha: REAL := 1.0;    (* Reflecton coefficient. *)
    Beta: REAL := 0.5;     (* Interpolation coefficient. *)
    Gamma: REAL := 1.0;    (* Extrapolation coefficient. *)
    Delta: REAL := 0.0;    (* Min-bias coefficient. *)
  ) =
 BEGIN
    WITH
      M = NUMBER(x),
      N = NUMBER(x[0]),
      
      xb = NEW(REF ARRAY OF REAL, N)^,   (* The simplex's barycenter *)

      xr = NEW(REF ARRAY OF REAL, N)^,   (* Work array used by LineSearch *)
      xs = NEW(REF ARRAY OF REAL, N)^    (* Work array used by LineSearch *)
    DO
      VAR nh: CARDINAL; (* Index of next-highest element *)
      
      VAR calls: CARDINAL := 0;
      
      PROCEDURE CallF (VAR z: ARRAY OF REAL): REAL =
        (* Calls normalize on "z", then evaluates "f(z)". *)
        BEGIN
          IF normalize # NIL THEN normalize(z) END;
          INC(calls);
          WITH fz = f(z) DO
            RETURN fz
          END
        END CallF;
      
      PROCEDURE ComputeAllYs () =
        (* Computes "y[i]" for all "i". *)
        BEGIN
          FOR i := 0 TO N DO y[i] := CallF(x[i]) END;
        END ComputeAllYs;
        
      PROCEDURE ComputeFaceCentroid () =
        (* 
          Computes the barycenter "xb" of the opposite face,
          biased towards the lowest vertex. 
        *)
        BEGIN
          FOR j := 0 TO N-1 DO xb[j] := 0.0 END;
          FOR i := 0 TO M-1 DO 
            IF i # hi THEN
              WITH xi = x[i] DO
                FOR j := 0 TO N-1 DO
                  xb[j] := xb[j] + xi[j]
                END;
              END
            END
          END;
          WITH 
            wl = Delta * FLOAT(M-1),
            wt = 1.0/(FLOAT(M-1) + wl),
            xl = x[lo]
          DO
            FOR j := 0 TO N-1 DO 
              xb[j] := wt * (wl * xl[j] + xb[j])
            END
          END;
        END ComputeFaceCentroid;
        
      VAR shrunk: BOOLEAN;
      
      VAR radius: REAL;
      
      PROCEDURE MinStep () =
        (* 
          Searches along the line from "x[hi]" through "xb", replacing
          "x[hi]" by any point found that is better than "x[hi]".
          
          If it can't find such a point, or if the point found is
          still the highest, contracts the simplex towards the lowest
          vertex. In that case, sets "shrunk" to TRUE, and stores in
          "radius" the length of the longest edge incident to the
          minimum vertex.
        *)
        VAR yr, ys: REAL := 0.0;
        BEGIN
          ComputeFaceCentroid ();
          shrunk := FALSE;
          (* Reflect: *)
          WITH xh = x[hi] DO
            FOR j := 0 TO N-1 DO 
              xr[j] := (1.0 + Alpha) * xb[j] -  Alpha * xh[j];
            END;
          END;
          yr := CallF(xr);

          IF yr < y[lo] THEN
            (* Extrapolate: *)
            FOR j := 0 TO N-1 DO
              xs[j] := Gamma * xr[j] + (1.0 - Gamma) * xb[j];
            END;
            ys := CallF(xs);

            IF ys < y[lo] THEN 
              IF verbose THEN Message("extrapolated, lowest") END;
              x[hi] := xs; y[hi] := ys
            ELSE 
              IF verbose THEN Message("reflected, lowest") END;
              x[hi] := xr; y[hi] := yr
            END
          ELSIF M > 2 AND yr < y[nh] THEN
            IF verbose THEN Message("reflected, middling") END;
            x[hi] := xr; y[hi] := yr;
          ELSE
            IF yr < y[hi] THEN
              IF verbose THEN Message("reflected, highest") END;
              x[hi] := xr; y[hi] := yr;
            END;

            (* Interpolate *)
            WITH xh = x[hi] DO
              FOR j := 0 TO N-1 DO
                xs[j] := Beta * xh[j] + (1.0 - Beta) * xb[j];
              END;
            END;
            ys := CallF(xs);

            IF ys < y[hi] THEN
              IF verbose THEN Message("interpolated") END;
              x[hi] := xs; y[hi] := ys
            ELSE
              IF verbose THEN Message("got stuck") END;
              ShrinkSimplex ();
              shrunk := TRUE
            END;
          END;
        END MinStep;
        
      PROCEDURE ShrinkSimplex () =
        (* 
          Computes the lemgth "dm" of the longest edge incident 
          to the minimum vertex, and shrinks simplex towards that
          vertex.
        *)
        VAR dm2, di2: LONGREAL;
        BEGIN
          WITH xl = x[lo] DO
            dm2 := 0.0D0;
            FOR i := 0 TO M-1 DO
              IF i # lo THEN
                di2 := 0.0D0;
                WITH xi = x[i] DO
                  FOR j := 0 TO N-1 DO
                    WITH 
                      xij = xi[j], 
                      xlj = xl[j], 
                      dij = FLOAT(xij - xlj, LONGREAL)
                    DO
                      xij := 0.5 * xij + 0.5 * xlj;
                      di2 := di2 + dij*dij;
                    END;
                  END;
                  y[i] := CallF(xi);
                  IF di2 > dm2 THEN dm2 := di2 END;
                END;
              END;
            END;
          END;
          radius := FLOAT(Math.sqrt(dm2));
        END ShrinkSimplex;

      PROCEDURE FindExtrema () =
        (* Sets "hi", "nx", and "lo" to point to max, second max, and min vertices. *)
        BEGIN
          FOR i := 0 TO M-1 DO 
            IF i = 0 OR y[i] < y[lo] THEN 
              lo := i 
            END;
            IF i = 0 THEN
              hi := i
            ELSIF y[i] >= y[hi] THEN 
              nh := hi; hi := i 
            ELSIF i = 1 OR y[i] > y[nh] THEN
              nh := i
            END;
          END;
          <* ASSERT nh # hi *>
        END FindExtrema;
      
      PROCEDURE Message (msg: TEXT) =
        <* FATAL Wr.Failure, Thread.Alerted *>
        BEGIN
          Wr.PutText(stderr, msg);
          Wr.PutText(stderr, "\n");
        END Message;
      
      PROCEDURE PrintStatus () =
        <* FATAL Wr.Failure, Thread.Alerted *>
        BEGIN 
          Wr.PutText(stderr, "\n");
          Wr.PutText(stderr, "calls = " & Fmt.Pad(Fmt.Int(calls), 9));
          Wr.PutText(stderr, "\n");
          PrintPoint("lo", y[lo], x[lo]);
          FOR i := 0 TO M-1 DO
            IF i # hi AND i # lo THEN
              PrintPoint("  ", y[i], x[i]);
            END;
          END;
          PrintPoint("hi", y[hi], x[hi]);
          Wr.PutText(stderr, "\n");
        END PrintStatus;

      PROCEDURE PrintPoint (msg: TEXT; fz: REAL; READONLY z: ARRAY OF REAL) =
        <* FATAL Wr.Failure, Thread.Alerted *>
        BEGIN 
          Wr.PutText(stderr, msg & " = " & Fmt.Pad(Fmt.Real(fz), 12) & "   ");
          Wr.PutText(stderr, "  ( ");
          FOR j := 0 TO N-1 DO 
            Wr.PutText(stderr, " ");
            Wr.PutText(stderr, Fmt.Pad(Fmt.Real(z[j]), 10));
          END;
          Wr.PutText(stderr, " )\n");
        END PrintPoint;

      VAR oldMin: REAL;

      BEGIN
        <* ASSERT M <= N+1 *>
        <* ASSERT M >= 1 *>
        ComputeAllYs ();
        FindExtrema ();
        IF report # NIL THEN
          IF report(x[lo], y[lo]) THEN RETURN END;
        END;
        IF M > 1 THEN
          WHILE calls < maxCalls AND y[lo] < y[hi] DO
            IF verbose THEN PrintStatus () END;
            oldMin := y[lo];
            MinStep ();
            FindExtrema ();
            IF y[lo] < oldMin THEN 
              IF report # NIL THEN
                IF report(x[lo], y[lo]) THEN RETURN END
              END;
            END;
            IF shrunk AND radius <= minSize THEN RETURN END;
          END;
        END
      END
    END
  END Minimize;

BEGIN
  
END SimMin.