MODULE SPHInterpolate;

IMPORT Wr, Fmt, Thread;
IMPORT LR3x3, LR3;
FROM GaussElimLongReal IMPORT Triangularize;
IMPORT 
  SPTriang, SPFunction, SPPWFunction, SPBezierPWFunction, SPDeCasteljau,
  SPHomoLabel;
FROM SPBezierPWFunction IMPORT TriangleData, ControlValues;
FROM SPHomoLabel IMPORT LabelToIndex, IndexToLabel;
FROM Stdio IMPORT stderr;

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

TYPE
  LONG = LONGREAL;
  NAT = CARDINAL;
  Triangles = ARRAY OF SPPWFunction.TriangleData;
  Label = SPHomoLabel.T;

PROCEDURE HInterpolateD4CN(
    func: SPFunction.T;
    tri: REF SPTriang.T;
  ): SPBezierPWFunction.T =
  CONST
    deg = 4;
    NC = (deg + 1) * (deg + 2) DIV 2;
    NP = NC;
  BEGIN
    WITH 
      NT = NUMBER(tri.side^),
      (* The function: *)
      spline = NEW(SPBezierPWFunction.T),
      (* Work vectors: *)
      p = NEW(REF ARRAY OF LR3.T, NP)^,
      f = NEW(REF ARRAY OF LONG, NP)^
    DO
     spline.tri:= tri;
     spline.data := MakeHTriangles(NT, NC);
     FOR k := 0 TO NT-1 DO
        WITH
          t = NARROW(spline.data[k], TriangleData)
        DO
          t.face := k;
          t.deg := deg;
          WITH
            c = t.c^
          DO
            (* Wr.PutText(stderr, "triangle " & FI(k) & "\n"); *)
            ComputeHProbePointsD4CN(tri.b2c[k], p);
            FOR s := 0 TO LAST(p) DO f[s] := func.eval(p[s]) END;
            HInterpolateTriangD4CN(tri.c2b[k], p, f, c)
          END
        END
      END;
      RETURN spline
    END
  END HInterpolateD4CN;

PROCEDURE ComputeHProbePointsD4CN(
    READONLY b2c: LR3x3.T;
    VAR (*OUT*) p: ARRAY OF LR3.T
  ) =
  (*
    Computes the probe points for H^4_{-1} interpolation 
    in a spherical triangle, with corners given by the matrix "b2c".
  *)
  CONST
    deg = 4;
    NC = (deg + 1) * (deg + 2) DIV 2;
    NP = NC;
  VAR b: LR3.T; 
  VAR s: CARDINAL := 0;
  CONST
    alpha1 = 0.9755282580d0;
    beta1  = 0.0122358710d0;
    gamma1 = 0.0122358710d0;
  CONST        
    alpha2 = 0.7938926260d0;
    beta2  = 0.0301837261d0;
    gamma2 = 0.1759236479d0;
  CONST        
    alpha3 = 0.5000000000d0;
    beta3  = 0.0334936490d0;
    gamma3 = 0.7165063510d0;
  CONST        
    alpha4 = 0.5000000000d0;
    beta4  = 0.2500000000d0;
    gamma4 = 0.2500000000d0;
  BEGIN
    <* ASSERT NUMBER(p) = NP *>
    FOR i := 0 TO 2 DO
      WITH j = (i+1) MOD 3, k = (i+2) MOD 3 DO
        b[i] := alpha1; b[j] := beta1; b[k] := gamma1;
        p[s] := LR3.Dir(LR3x3.MapCol(b2c, b)); INC(s);
        
        b[i] := alpha2; b[j] := beta2; b[k] := gamma2;
        p[s] := LR3.Dir(LR3x3.MapCol(b2c, b)); INC(s);
        
        b[i] := alpha2; b[j] := gamma2; b[k] := beta2;
        p[s] := LR3.Dir(LR3x3.MapCol(b2c, b)); INC(s);
        
        b[i] := alpha3; b[j] := gamma3; b[k] := beta3;
        p[s] := LR3.Dir(LR3x3.MapCol(b2c, b)); INC(s);
        
        b[i] := alpha4; b[j] := beta4; b[k] := gamma4;
        p[s] := LR3.Dir(LR3x3.MapCol(b2c, b)); INC(s);
      END
    END;
    <* ASSERT s = NP *>
  END ComputeHProbePointsD4CN;
  
PROCEDURE HInterpolateTriangD4CN(
    READONLY c2b: LR3x3.T;
    READONLY p: ARRAY OF LR3.T;
    READONLY f: ARRAY OF LONG;
    (* OUT: *)
    VAR c: ARRAY OF LONG;
  ) =
  CONST
    deg = 4;
    NC = (deg + 1) * (deg + 2) DIV 2;
    NP = NC;
  VAR 
    A: ARRAY [0..NP-1] OF ARRAY [0..NC] OF LONGREAL;
    x: ARRAY [0..NC-1] OF LONGREAL;
  BEGIN
    <* ASSERT NUMBER(p) = NP *>
    <* ASSERT NUMBER(f) = NP *>
    <* ASSERT NUMBER(c) = NC *>
    FOR s := 0 TO LAST(p) DO
      WITH
        bpf = LR3x3.MapCol(c2b, p[s])
      DO
        FOR ijk := 0 TO NC-1 DO
          WITH lbl = IndexToLabel(ijk, deg) DO
            A[s,ijk] := BezierValueDim2(lbl[0], lbl[1], lbl[2], bpf)
          END
        END;
        (* Independent term: *)
        A[s,NC] := f[s]
      END
    END;

    (* Solve system for the coefficients associated with edge "ie" *)
    (* PrintSystem("coefs", A); *)
    Triangularize(A);
    BackSolve(A, x);
    (* PrintSolution("coefs", x); *)
    FOR ijk := 0 TO NC-1 DO
      c[ijk] := x[ijk];
    END;
    (* PrintCoeffs(c, 3); *)
  END HInterpolateTriangD4CN;

PROCEDURE HInterpolateD4C0(
    func: SPFunction.T;
    tri: REF SPTriang.T;
  ): SPBezierPWFunction.T =
  BEGIN
    WITH 
      deg = 4,
      NT = NUMBER(tri.side^),
      NC = (deg + 1) * (deg + 2) DIV 2,
      NP = 15, (* Number of probe points per triangle *)
      (* The function: *)
      spline = NEW(SPBezierPWFunction.T),
      (* Work vectors: *)
      p = NEW(REF ARRAY OF LR3.T, NP)^,
      f = NEW(REF ARRAY OF LONG, NP)^
    DO
     spline.tri:= tri;
     spline.data := MakeHTriangles(NT, NC);
     FOR k := 0 TO NT-1 DO
        WITH
          t = NARROW(spline.data[k], TriangleData)
        DO
          t.face := k;
          t.deg := deg;
          WITH c = t.c^ DO
            ComputeHProbePointsD4C0(tri.b2c[k], p);
            FOR s := 0 TO LAST(p) DO f[s] := func.eval(p[s]) END;
            HInterpolateTriangD4C0(tri.c2b[k], p, f, c)
          END
        END
      END;
      RETURN spline
    END
  END HInterpolateD4C0;
  
PROCEDURE ComputeHProbePointsD4C0(
    READONLY b2c: LR3x3.T;
    VAR (*OUT*) p: ARRAY OF LR3.T
  ) =
  (*
    Computes the probe points for H^4_0
    interpolation in a spherical triangle,
    with corners given by the matrix "b2c".
  *)
  VAR b: LR3.T; 
  VAR s: CARDINAL := 0;
  CONST NP = 15;
  CONST
    alpha = 0.75d0;
    beta =  0.25d0;
  CONST
    gamma = 0.50d0;
    delta = 0.25d0;
    epsilon = 0.25d0;
  BEGIN
    (* Sample points at corners: *)
    FOR i := 0 TO 2 DO
      b := LR3.T{0.0d0, 0.0d0, 0.0d0};
      b[i] := 1.0d0;
      p[s] := LR3.Dir(LR3x3.MapCol(b2c, b)); INC(s);
    END;
    (* Sample points along edges: *)
    FOR i := 0 TO 2 DO
      WITH j = (i+1) MOD 3, k = (i+2) MOD 3 DO
        b[i] := 0.0d0; b[j] := alpha; b[k] := beta;
        p[s] := LR3.Dir(LR3x3.MapCol(b2c, b)); INC(s);
        b[i] := 0.0d0; b[j] := 0.50d0; b[k] := 0.50d0;
        p[s] := LR3.Dir(LR3x3.MapCol(b2c, b)); INC(s);
        b[i] := 0.0d0; b[j] := beta; b[k] := alpha;
        p[s] := LR3.Dir(LR3x3.MapCol(b2c, b)); INC(s);
      END
    END;
    (* Sample point inside face: *)
    FOR i := 0 TO 2 DO
      WITH j = (i+1) MOD 3, k = (i+2) MOD 3 DO
        b[i] := gamma; b[j] := delta; b[k] := epsilon;
        p[s] := LR3.Dir(LR3x3.MapCol(b2c, b)); INC(s);
      END
    END;
    <* ASSERT s = NP *>
  END ComputeHProbePointsD4C0;
  
PROCEDURE HInterpolateTriangD4C0(
    READONLY c2b: LR3x3.T;
    READONLY p: ARRAY OF LR3.T;
    READONLY f: ARRAY OF LONG;
    (* OUT: *)
    VAR c: ARRAY OF LONG;
  ) =
  VAR A: LR3x3.T; b: LR3.T;
  BEGIN
    FOR ijk := 0 TO LAST(c) DO c[ijk] := 0.0d0 END;
    (* Compute the vertex-associated coefficients: *)
    c[ 0] := f[0];
    c[10] := f[1];
    c[14] := f[2];
    (* Compute the edge-associated coefficients: *)
    FOR ie := 0 TO 2 DO
      WITH je = (ie+1) MOD 3, ke = (ie+2) MOD 3 DO
        (* Solve the coefficients associated with edge "ie" *)
        FOR sx := 0 TO 2 DO
          WITH
            s = 3 + 3*ie + sx,
            a = LR3x3.MapCol(c2b, p[s])
          DO
            A[sx,0] := BezierValueDim1(3,1, a[je], a[ke]);
            A[sx,1] := BezierValueDim1(2,2, a[je], a[ke]);
            A[sx,2] := BezierValueDim1(1,3, a[je], a[ke]);
            b[sx] := f[s] - SPDeCasteljau.Eval4(c, a)
          END
        END;
        WITH
          x = LR3x3.MapCol(LR3x3.Inv(A), b)
        DO
          c[CoeffIndex(0,3,1, ie)] := x[0];
          c[CoeffIndex(0,2,2, ie)] := x[1];
          c[CoeffIndex(0,1,3, ie)] := x[2]
        END
      END
    END;
    
    (* Compute the face-associated coefficients: *)
    FOR ie := 0 TO 2 DO
      WITH 
        s = 12 + ie,
        a = LR3x3.MapCol(c2b, p[s])
      DO
        A[ie,0] := BezierValueDim2(2,1,1, a);
        A[ie,1] := BezierValueDim2(1,2,1, a);
        A[ie,2] := BezierValueDim2(1,1,2, a);
        b[ie] := f[s] - SPDeCasteljau.Eval4(c, a)
      END
    END;
    WITH
      x = LR3x3.MapCol(LR3x3.Inv(A), b)
    DO
      c[LabelToIndex(Label{2,1,1}, 4)] := x[0];
      c[LabelToIndex(Label{1,2,1}, 4)] := x[1];
      c[LabelToIndex(Label{1,1,2}, 4)] := x[2]
    END;
    (* PrintCoeffs(c, 4); *)
  END HInterpolateTriangD4C0;
  
<*UNUSED*>
PROCEDURE PrintCoeffs(READONLY c: ARRAY OF LONG; deg: NAT) =
  BEGIN
    FOR i := 0 TO deg DO
      FOR j := 0 TO deg-i DO
        WITH 
          k = deg-i-j,
          ijk = LabelToIndex(Label{i,j,k},deg)
        DO 
          IF c[ijk] # 0.0d0 THEN
            Wr.PutText(stderr, "c[" & FI(i) & "," & FI(j) & "," & FI(k) & "] = " & FLR(c[ijk],8,5) & "\n");
          END
        END 
      END
    END
  END PrintCoeffs;
  
PROCEDURE CoeffIndex(i,j,k, perm: CARDINAL): CARDINAL =
  (*
    Index into the appropriate Bezier coefficient vector
    of the coefficient "c_{i',j',k'}", where "(i',j'.k')"
    is "(i,j,k)" rotated forward (to the right) by "perm" places. *)
  BEGIN
    WHILE perm > 0 DO 
      VAR t := k; BEGIN k := j; j := i; i := t END; DEC(perm)
    END;
    RETURN LabelToIndex(Label{i,j,k}, i+j+k)
  END CoeffIndex;

PROCEDURE BezierValueDim1(i, j: CARDINAL; x, y: LONG): LONG =
  (*
    Bezier polynomial "B^{i+j}_{i,j}(x,y)" *)
  VAR B: LONG;
  BEGIN
    B := 1.0d0;
    FOR r := 1 TO i DO
      B := B * x;
    END;
    FOR s := 1 TO j DO 
      B := B * y * FLOAT(i + s, LONG)/FLOAT(s, LONG)
    END;
    RETURN B
  END BezierValueDim1;
  
PROCEDURE BezierValueDim2(i,j,k: CARDINAL; READONLY p: LR3.T): LONG =
  (*
    Bezier polynomial "B^{i+j+k}_{i,j,k}" at barycentric
    coordinates "p" *)
  VAR B: LONG;
  BEGIN
    B := 1.0d0;
    FOR r := 1 TO i DO
      B := B * p[0];
    END;
    FOR s := 1 TO j DO 
      B := B * p[1] * FLOAT(i + s, LONG)/FLOAT(s, LONG)
    END;
    FOR t := 1 TO k DO 
      B := B * p[2] * FLOAT(i + j + t, LONG)/FLOAT(t, LONG)
    END;
    RETURN B
  END BezierValueDim2;

PROCEDURE MakeHTriangles(
    n: CARDINAL; (* Number of triangles *)
    NC: CARDINAL; 
  ): REF Triangles =
  (*
    Allocates an array of "n" triangle coefficient vectors 
    for a homogeneous spherical spline of degree "deg".
    Assumes that it has "NC" coefficients per triangle. *)
  BEGIN
    WITH
      r = NEW(REF Triangles, n),
      t = r^
    DO
      FOR i := 0 TO n-1 DO 
        WITH ti = NEW(TriangleData) DO
          t[i] := ti;
          ti.barycentric := TRUE;
          ti.c := NEW(REF ControlValues, NC);
          WITH c = ti.c^ DO 
            FOR i := 0 TO NC-1 DO c[i] := 0.0d0 END
          END;
        END
      END;
      RETURN r
    END
  END MakeHTriangles;

PROCEDURE BackSolve(
    READONLY A: ARRAY OF ARRAY OF LONG;
    VAR (*OUT*) x: ARRAY OF LONG;
  ) =
  (*
    Solves the linear system "A*x = b" where "A" is
    upper triangular. *)
  VAR sum: LONG;
  BEGIN
    WITH N = NUMBER(x) DO
      <* ASSERT NUMBER(A) = N *>
      <* ASSERT NUMBER(A[0]) = N+1 *>
      FOR i := N-1 TO 0 BY -1 DO
        sum := 0.0d0;
        FOR j := i+1 TO N-1 DO sum := sum + A[i,j]*x[j] END;
        x[i] := (A[i,N] - sum)/A[i,i]
      END
    END
  END BackSolve;

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 FI(x: INTEGER; wid: CARDINAL := 0): TEXT =
  BEGIN
    RETURN Fmt.Pad(Fmt.Int(x), wid)
  END FI;

BEGIN
END SPHInterpolate.