MODULE CsiFeatures2D;

IMPORT Wr, Thread;
IMPORT FeatureMapper2D AS Mapper;
FROM Mapper IMPORT
  NAT, Nhood, Nhoods, CoNhoods, Features, CoFeatures,
  SpaceDerivatives, EvalSpaceDerivatives, DiffSpaceDerivatives;

REVEAL
  T = Public BRANDED OBJECT
    OVERRIDES
      init := Init;
      print := Print;
      eval := Eval;
      diff := Diff;
      defined := Defined;
    END; 

PROCEDURE Init(s: T; NL: NAT): T =
  BEGIN
    s.NL := NL;
    s.NF := 5*NL;
    RETURN s
  END Init;

PROCEDURE Print(<*UNUSED*> s: T; wr: Wr.T) =
  <* FATAL Wr.Failure, Thread.Alerted *>
  BEGIN
    Wr.PutText(wr, "Csi: the csi invariants\n");
    Wr.Flush(wr)
  END Print;

PROCEDURE Eval(s: T; READONLY n: Nhoods; VAR t: Features) =
  BEGIN
    FOR i := 0 TO s.NL-1 DO
      WITH
        n = n[i],
        t = SUBARRAY(t, 5*i, 5),

        D = EvalSpaceDerivatives(n),
        fx2 = D.fx*D.fx,
        fy2 = D.fy*D.fy
      DO
        t[0] := D.f;
        t[1] := fx2 + fy2;
        t[2] := D.fxx + D.fyy;
        t[3] := D.fxx*D.fyy - D.fxy*D.fxy;
        t[4] := D.fxx*fx2 + 2.0d0*D.fx*D.fy*D.fxy + D.fyy*fy2;
      END
    END
  END Eval;

PROCEDURE Diff(
    s: T;
    READONLY dEdt: CoFeatures;
    <*UNUSED*> READONLY t: Features;
    READONLY n: Nhoods;
    VAR dEdn: CoNhoods;
  ) =
  BEGIN
    FOR i := 0 TO s.NL-1 DO
      WITH
        dEdt = SUBARRAY(dEdt, 5*i, 5),
        n = n[i],
        dEdn = dEdn[i],

        D = EvalSpaceDerivatives(n),
        fx2 = D.fx*D.fx,
        fy2 = D.fy*D.fy,

        dEdD = SpaceDerivatives{
          f := dEdt[0],
          fx := 2.0d0 * (dEdt[1]*D.fx + dEdt[4]*(D.fxx*D.fx + D.fxy*D.fy)),
          fy := 2.0d0 * (dEdt[1]*D.fy + dEdt[4]*(D.fyy*D.fx + D.fxy*D.fy)),
          fxx := dEdt[2] + dEdt[3]*D.fyy + dEdt[4]*fx2,
          fxy := 2.0d0*(dEdt[3]*D.fxy + dEdt[4]*D.fx*D.fy),
          fyy := dEdt[2] + dEdt[3]*D.fxx + dEdt[4]*fy2
        }
      DO
        DiffSpaceDerivatives(dEdD, dEdn)
      END
    END
  END Diff;

PROCEDURE Defined(
    <*UNUSED*> s: T;
    READONLY m: Nhood
  ): BOOLEAN =
  BEGIN
    RETURN
      m[-1,-1] > 0.5 AND
      m[-1,00] > 0.5 AND
      m[-1,+1] > 0.5 AND
      m[00,-1] > 0.5 AND
      m[00,00] > 0.5 AND
      m[00,+1] > 0.5 AND
      m[+1,-1] > 0.5 AND
      m[+1,00] > 0.5 AND
      m[+1,+1] > 0.5
  END Defined;

BEGIN
END CsiFeatures2D.