MODULE PGMImageFilter;
(* Last edited on 1999-10-30 12:56:08 by stolfi *)

IMPORT PGM, Rd, Wr, Thread, LongFloat, FloatMode;
FROM Math IMPORT sqrt;

<* FATAL Rd.Failure, Wr.Failure, Thread.Alerted, FloatMode.Trap *>

PROCEDURE ReadPixels(
    imRd: PGM.Reader;              (* Image reader. *)
    VAR im: ARRAY OF NAT;          (* Pixels to read. *)
    mkRd: PGM.Reader := NIL;       (* Optional `mask' image reader. *)
    noPixel: NAT := LAST(NAT);     (* `Null' pixel value. *)            
  ) =
  VAR mkPix: NAT;
  BEGIN
    FOR x := 0 TO LAST(im) DO 
      WITH imPix = im[x] DO
        imRd.get(imPix);
        IF mkRd # NIL THEN 
          mkRd.get(mkPix);
          IF mkPix = 0 THEN imPix := noPixel END
        END
      END
    END;
  END ReadPixels;
  
PROCEDURE WritePixels(
    imWr: PGM.Writer;              (* Image reader. *)                  
    READONLY im: ARRAY OF Pixel;   (* Pixels to write. *)                
    mkWr: PGM.Writer := NIL;       (* Optional `mask' image reader. *)  
    noPixel: NAT := LAST(NAT);     (* `Null' pixel value. *)            
  ) =
  BEGIN 
    FOR x := 0 TO LAST(im) DO
      WITH imPix = im[x] DO
        IF mkWr # NIL THEN
          IF imPix = noPixel THEN
            mkWr.put(0); imWr.put(0)
          ELSE
            mkWr.put(1); imWr.put(imPix)
          END
        ELSE
          imWr.put(imPix)
        END
      END
    END;
  END WritePixels;

PROCEDURE ReadPixelBufferRow(
    imRd: PGM.Reader;          (* Image reader. *)
    yp: INTEGER;               (* "Y" coordinate of scanline to read. *)
    VAR imBuf: PixelBuffer;    (* Cyclic pixel row buffer. *)
    NY: NAT;                   (* Number of rows in image. *)
    mkRd: PGM.Reader := NIL;   (* Optional `mask' image reader. *)
    noPixel: NAT := LAST(NAT); (* `Null' pixel value. *)   
  ) = 
  BEGIN
    IF yp >= 0 AND yp <= NY-1 THEN
      WITH kp = yp MOD NUMBER(imBuf) DO
        ReadPixels(imRd, imBuf[kp], mkRd, noPixel)
      END
    END
  END ReadPixelBufferRow;

PROCEDURE PixelRange(maxPixel: NAT; noPixel: NAT): NAT =
  BEGIN
    IF noPixel <= maxPixel THEN
      RETURN maxPixel - 1
    ELSE
      RETURN maxPixel
    END
  END PixelRange;
  
PROCEDURE FloatPixels(
    READONLY im: ARRAY OF NAT;      (* Integer pixels. *)
    maxPixel: NAT;                  (* Maximum pixel value, as declared in PGM file. *)
    VAR fm: ARRAY OF Interval;      (* Converted pixels. *)
    noPixel: NAT;                   (* `Null' pixel value. *)
  ) =
  VAR p: NAT;
  BEGIN
    <* ASSERT NUMBER(im) = NUMBER(fm) *>
    WITH 
      range = PixelRange(maxPixel, noPixel),
      scale = 1.0d0/FLOAT(range, LONG)
    DO
      FOR xp := 0 TO LAST(im) DO
        p := im[xp];
        IF p = noPixel THEN
          fm[xp] := Interval{0.0d0, 1.0d0}
        ELSE
          <* ASSERT p <= maxPixel *>
          IF p > noPixel THEN DEC(p) END;
          WITH s = FLOAT(p, LONG) DO
            fm[xp] := Interval{
              MAX(0.0d0, (s - 0.5d0) * scale), 
              MIN(1.0d0, (s + 0.5d0) * scale)
            }
          END
        END
      END
    END
  END FloatPixels;

PROCEDURE FixLongPixels(
    READONLY fm: ARRAY OF LONG;  (* Pixel value ranges. *)
    maxPixel: NAT;               (* Maximum pixel value, as in PGM file. *)
    VAR im: ARRAY OF NAT;        (* Integer pixels. *)
    noPixel: NAT := LAST(NAT);   (* `Null' pixel value. *)
  ) =
  VAR p: NAT;
  BEGIN
    <* ASSERT NUMBER(im) = NUMBER(fm) *>
    WITH 
      range = PixelRange(maxPixel, noPixel),
      scale = FLOAT(range, LONG)
    DO
      FOR xp := 0 TO LAST(im) DO
        WITH s = MIN(1.0d0, MAX(0.0d0, fm[xp])) DO
          p := ROUND(s * scale);
        END;
        IF p >= noPixel THEN INC(p) END;
        <* ASSERT p <= maxPixel *>
        im[xp] := p
      END
    END
  END FixLongPixels;
  
PROCEDURE FixIntervalPixels(
    READONLY fm: ARRAY OF Interval;  (* Pixel value ranges. *)
    maxPixel: NAT;                   (* Maximum pixel value, as in PGM file. *)
    maxError: LONG;                  (* Maximum error tolerance. *)
    VAR im: ARRAY OF NAT;            (* Integer pixels. *)
    noPixel: NAT := LAST(NAT);       (* `Null' pixel value. *)
  ) =
  VAR p: NAT;
  BEGIN
    <* ASSERT NUMBER(im) = NUMBER(fm) *>
    WITH 
      range = PixelRange(maxPixel, noPixel),
      scale = FLOAT(range, LONG),
      maxWidth = maxError + maxError
    DO
      FOR xp := 0 TO LAST(im) DO
        WITH s = fm[xp] DO
          IF s.lo > s.hi OR s.hi - s.lo > maxWidth THEN
            p := noPixel
          ELSE
            WITH c = MIN(1.0d0, MAX(0.0d0, 0.5d0*(s.lo + s.hi))) DO
              p := ROUND(c * scale)
            END;
            IF p >= noPixel THEN INC(p) END;
            <* ASSERT p <= maxPixel *>
          END;
          im[xp] := p
        END
      END
    END
  END FixIntervalPixels;
  
PROCEDURE FixIntervalPixelErrors(
    READONLY fm: ARRAY OF Interval;  (* Pixel value ranges. *)
    maxPixel: NAT;                   (* Maximum pixel value, as in PGM file. *)
    VAR im: ARRAY OF NAT;            (* Integer pixels. *)
    noPixel: NAT := LAST(NAT);       (* `Null' pixel value. *)
  ) =
  VAR p: NAT;
  BEGIN
    <* ASSERT NUMBER(im) = NUMBER(fm) *>
    WITH 
      range = PixelRange(maxPixel, noPixel),
      scale = FLOAT(range, LONG)
    DO
      FOR xp := 0 TO LAST(im) DO
        WITH s = fm[xp] DO
          IF s.lo > s.hi THEN
            p := noPixel
          ELSE
            WITH c = MIN(1.0d0, MAX(0.0d0, s.hi - s.lo)) DO
              p := ROUND(c * scale)
            END;
            IF p >= noPixel THEN INC(p) END;
            <* ASSERT p <= maxPixel *>
          END;
          im[xp] := p
        END
      END
    END
  END FixIntervalPixelErrors;
  
PROCEDURE ReplacePixelValue(a, b: NAT;VAR im: ARRAY OF NAT) =
  BEGIN
    FOR xp := 0 TO LAST(im) DO 
      IF im[xp] = a THEN im[xp] := b END
    END
  END ReplacePixelValue;

PROCEDURE WeightedHistogram(
    xp, yp: NAT;
    READONLY imBuf: PixelBuffer;
    NX, NY: NAT;
    READONLY w: ARRAY OF ARRAY OF LONG; 
    VAR hist: Histogram;
  ) =
  BEGIN
    <* ASSERT yp < NY *>
    <* ASSERT xp < NX *>
    WITH 
      NWX = NUMBER(w[0]), HWX = NWX DIV 2,
      NWY = NUMBER(w),    HWY = NWY DIV 2
    DO
      <* ASSERT NWX MOD 2 = 1 *>
      <* ASSERT NWY MOD 2 = 1 *>
      <* ASSERT NUMBER(imBuf) >= NWY *>
      <* ASSERT NUMBER(imBuf[0]) = NX *>
      FOR q := 0 TO LAST(hist) DO hist[q] := 0.0d0 END;
      FOR yw := -MIN(yp, HWY) TO MIN(NY-1-yp, HWY) DO
        WITH 
          yq = yp + yw, 
          kq = yq MOD NUMBER(imBuf),
          imy = imBuf[kq],
          wy = w[yw + HWY]
        DO
          FOR xw := -MIN(xp, HWX) TO MIN(NX-1-xp, HWX) DO
            WITH xq = xp + xw, q = imy[xq] DO
              IF q <= LAST(hist) THEN 
                hist[q] := hist[q] + wy[xw + HWX]
              END
            END
          END
        END
      END;
    END
  END WeightedHistogram;

PROCEDURE RankFromHistogram(
    p: NAT;
    READONLY hist: Histogram;
    fuzz: LONG := 0.0d0;
  ): Interval =
  VAR hSum, shSum: LONG := 0.0d0;
  BEGIN
    WITH ff = fuzz + fuzz DO
      FOR q := 0 TO LAST(hist) DO 
        WITH h = hist[q] DO
          IF h > 0.0d0 THEN
            WITH dd = FLOAT(p - q, LONG) DO
              IF dd = 0.0d0 THEN 
                shSum := shSum + 0.5d0 * h
              ELSIF dd >= ff THEN 
                shSum := shSum + h
              ELSIF dd <= -ff THEN 
                (* Null score *)
              ELSE
                WITH 
                  r = dd/ff,
                  score = 0.25d0 * r*(3.0d0 - r*r) + 0.5d0
                DO
                  shSum := shSum + score * h
                END
              END;
            END;
            hSum := hSum + h
          END
        END
      END;
      WITH 
        lo = MIN(1.0d0, shSum), 
        hi = MIN(1.0d0, shSum + MAX(0.0d0, 1.0d0 - hSum))
      DO
        RETURN Interval{lo, hi}
      END
    END
  END RankFromHistogram;
  
PROCEDURE MeanFromHistogram(READONLY hist: Histogram): LONG =
  VAR hSum, hqSum: LONG := 0.0d0;
  BEGIN
    FOR q := 0 TO LAST(hist) DO 
      WITH h = hist[q] DO 
        IF h > 0.0d0 THEN 
          hSum := hSum + h; hqSum := hqSum + h * FLOAT(q, LONG)
        END
      END
    END;
    IF hSum = 0.0d0 THEN 
      RETURN FLOAT(LAST(hist), LONG)/2.0d0
    ELSE
      RETURN hqSum / hSum
    END
  END MeanFromHistogram;

PROCEDURE DeviationFromHistogram(READONLY hist: Histogram; mean: LONG): LONG =
  VAR hSum, hddSum: LONG := 0.0d0;
  CONST QuantVar = 1.0d0/12.0d0;  (* Nominal variance of quantization error. *)
  BEGIN
    FOR q := 0 TO LAST(hist) DO 
      WITH h = hist[q] DO 
        IF h > 0.0d0 THEN 
          WITH d = FLOAT(q, LONG) - mean, dd = d*d DO
            hSum := hSum + h; hddSum := hddSum + h * dd
          END
        END
      END
    END;
    IF hSum = 0.0d0 THEN 
      WITH m = FLOAT(LAST(hist), LONG) DO
        hSum := 1.0d0;
        hddSum := mean*mean - m*mean + m*(m + m + 1.0d0)/6.0d0
      END
    END;
    RETURN sqrt(hddSum / hSum + QuantVar)
  END DeviationFromHistogram;

PROCEDURE GroundFromHistogram(READONLY hist: Histogram; alpha: LONG): LONG =
  VAR wSum, wqSum: LONG := 0.0d0;
      s: LONG; q: NAT;
  BEGIN
    IF alpha = 1.0d0 THEN
      (* Plain average *)
      FOR q := 0 TO LAST(hist) DO 
        WITH w = hist[q] DO
          IF w > 0.0d0 THEN
            wSum := wSum + w;
            wqSum := wqSum + w * FLOAT(q, LONG)
          END
        END;
      END;
      IF wSum > 0.0d0 THEN
        RETURN wqSum/wSum
      ELSE
        RETURN FLOAT(LAST(hist), LONG)/2.0d0
      END
    ELSE
      (* Exponentially weighted average, starting from low-weight END: *)
      WITH
        reverse = alpha < 1.0d0,
        eps = LongFloat.Scalb(1.0d0, -900),
        big = LongFloat.Scalb(1.0d0, +900)
      DO
        IF reverse THEN alpha := 1.0d0/alpha END;
        s := eps;
        FOR i := 0 TO LAST(hist) DO 
          IF reverse THEN q := LAST(hist) - i ELSE q := i END;
          WITH h = hist[q] DO
            IF h > 0.0d0 THEN
              WITH w = s*h DO
                wSum := wSum + w;
                wqSum := wqSum + w * FLOAT(q, LONG)
              END
            END
          END;
          s := s * alpha;
          <* ASSERT s <= big *>
        END;
        (* Now "s = eps*alpha^NUMBER(hist)" *)
        IF wSum > 0.0d0 THEN
          RETURN wqSum/wSum
        ELSE
          (* Fake it. Note that "s = eps*alpha^NUMBER(hist)" *)
          WITH
            m = FLOAT(LAST(hist), LONG),
            r = alpha/(alpha - 1.0d0),
            beta = ((alpha - 1.0d0)*m - 1.0d0)/alpha,
            avg = r * (beta * s + eps)/(alpha * s - eps)
          DO 
            IF reverse THEN 
              RETURN m - avg
            ELSE
              RETURN avg
            END
          END
        END
      END
    END
  END GroundFromHistogram;

PROCEDURE FractileFromHistogram(READONLY hist: Histogram; fraction: LONG): LONG =
  VAR wSum: LONG := 0.0d0;
      s: LONG := 0.0d0;
      lo, hi: LONG;
  BEGIN
    IF fraction <= 0.0d0 THEN RETURN 0.0d0 END;
    FOR i := 0 TO LAST(hist) DO wSum := wSum + hist[i] END;
    IF wSum <= 0.0d0 THEN RETURN 0.0d0 END;
    WITH
      fSum = fraction * wSum
    DO
      lo := 0.0d0;
      FOR q := 0 TO LAST(hist) DO 
        hi := FLOAT(q, LONG);
        IF q < LAST(hist) THEN hi := hi + 0.5d0 END;
        WITH h = hist[q] DO
          IF h > 0.0d0 THEN
            WITH sh = s + h DO
              IF sh = fSum THEN 
                RETURN hi
              ELSIF sh > fSum THEN
                RETURN lo + (hi - lo) * (fSum - s)/h
              ELSE
                s := sh
              END;
            END
          END
        END;
        lo := hi;
      END;
      RETURN lo;
    END
  END FractileFromHistogram;

PROCEDURE FilterRow(
    READONLY im: ARRAY OF Interval; 
    READONLY w: ARRAY OF LONG; 
    VAR om: ARRAY OF Interval;
    base: NAT := 0;
    step: NAT := 1;
    noPixel: Interval := Interval{0.0d0, 1.0d0};
  ) =

  VAR wloSum, whiSum: LONG;
      q: Interval;
  BEGIN
    WITH NW = NUMBER(w), HW = NW DIV 2 DO
      <* ASSERT NW MOD 2 = 1 *>
      FOR xf := 0 TO LAST(om) DO
        wloSum := 0.0d0; whiSum := 0.0d0;
        WITH xp = base + step*xf DO
          FOR r := -HW TO HW DO
            WITH xq = xp + r DO
              IF xq < 0 OR xq > LAST(im) THEN
                q := noPixel
              ELSE
                q := im[xq]
              END
            END;
            WITH W = w[r + HW] DO
              IF W >= 0.0d0 THEN
                wloSum := wloSum + W * q.lo; whiSum := whiSum + W * q.hi
              ELSE
                wloSum := wloSum + W * q.hi; whiSum := whiSum + W * q.lo
              END
            END
          END
        END;
        om[xf] := Interval{wloSum, whiSum}
      END
    END
  END FilterRow;

PROCEDURE MixRows(
    yp: NAT;
    READONLY imBuf: IntervalBuffer;
    NX, NY: NAT;
    READONLY w: ARRAY OF LONG; 
    VAR om: ARRAY OF Interval;
    noPixel: Interval := Interval{0.0d0, 1.0d0};
  ) =
  VAR wloSum, whiSum: LONG;
      q: Interval;
  BEGIN
    WITH NW = NUMBER(w), HW = NW DIV 2 DO
      <* ASSERT NW MOD 2 = 1 *>
      <* ASSERT NUMBER(imBuf) >= NW *>
      <* ASSERT NUMBER(imBuf[0]) = NX *>
      <* ASSERT NUMBER(om) = NX *>
      FOR x := 0 TO NX-1 DO 
        wloSum := 0.0d0; whiSum := 0.0d0;
        FOR r := -HW TO HW DO 
          WITH yq = yp + r DO
            IF yq < 0 OR yq >= NY THEN
              q := noPixel
            ELSE
              WITH kq = yq MOD NUMBER(imBuf) DO
                q := imBuf[kq,x]
              END
            END
          END;
          WITH W = w[r + HW] DO
            IF W >= 0.0d0 THEN
              wloSum := wloSum + W * q.lo; whiSum := whiSum + W * q.hi
            ELSE
              wloSum := wloSum + W * q.hi; whiSum := whiSum + W * q.lo
            END
          END
        END;
        om[x] := Interval{wloSum, whiSum}
      END
    END
  END MixRows;
  
PROCEDURE WeightedSum(
    xp, yp: NAT;
    READONLY imBuf: IntervalBuffer;
    NX, NY: NAT;
    READONLY w: ARRAY OF ARRAY OF LONG; 
    noPixel: Interval := Interval{0.0d0, 1.0d0};
  ): Interval =
  VAR wloSum, whiSum: LONG := 0.0d0;
      q: Interval;
  BEGIN
    <* ASSERT yp < NY *>
    <* ASSERT xp < NX *>
    WITH 
      NB = NUMBER(imBuf),
      NWX = NUMBER(w[0]), HWX = NWX DIV 2,
      NWY = NUMBER(w),    HWY = NWY DIV 2
    DO
      <* ASSERT NWX MOD 2 = 1 *>
      <* ASSERT NWY MOD 2 = 1 *>
      <* ASSERT NB >= NWY *>
      <* ASSERT NUMBER(imBuf[0]) = NX *>
      FOR yw := -HWY TO HWY DO
        WITH 
          wy = w[yw + HWY],
          yq = yp + yw
        DO
          FOR xw := -HWX TO HWX DO
            WITH 
              wxy = wy[xw + HWX],
              xq = xp + xw
            DO
              IF xq >= 0 AND xq < NX AND yq >= 0 AND yq < NY THEN
                q := imBuf[yq MOD NB, xq]
              ELSE
                q := noPixel
              END;
              IF wxy > 0.0d0 THEN
                wloSum := wloSum + wxy * q.lo;
                whiSum := whiSum + wxy * q.hi
              ELSE                     
                wloSum := wloSum + wxy * q.hi;
                whiSum := whiSum + wxy * q.lo
              END
            END
          END
        END
      END;
      RETURN Interval{wloSum, whiSum}
    END
  END WeightedSum;

BEGIN
END PGMImageFilter.