MODULE SPNHInterpolate;

IMPORT Wr, Fmt, Thread;
IMPORT LR3x3, LR3, LR3Extras;
FROM GaussElimLongReal IMPORT Triangularize;
IMPORT 
  SPTriang, SPFunction, SPPWFunction, SPBezPolyPWFunction, SPDeCasteljau,
  SPHomoLabel;
FROM SPBezPolyPWFunction 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 NHInterpolateD3CN(
    func: SPFunction.T;
    tri: REF SPTriang.T;
  ): SPBezPolyPWFunction.T =
  CONST
    deg = 3;
    NC0 = (deg + 1) * (deg + 2) DIV 2;
    NC1 = deg * (deg + 1) DIV 2;
    NP = NC0 + NC1;
  BEGIN
    WITH 
      NT = NUMBER(tri.side^),
      (* The function: *)
      spline = NEW(SPBezPolyPWFunction.T),
      (* Work vectors: *)
      p = NEW(REF ARRAY OF LR3.T, NP)^,
      f = NEW(REF ARRAY OF LONG, NP)^
    DO
     spline.tri:= tri;
     spline.data := MakeNHTriangles(NT, NC0, NC1);
     FOR k := 0 TO NT-1 DO
        WITH
          t = NARROW(spline.data[k], TriangleData)
        DO
          t.face := k;
          t.deg := deg;
          WITH
            c0 = t.c0^, c1 = t.c1^
          DO
            (* Wr.PutText(stderr, "triangle " & FI(k) & "\n"); *)
            ComputeNHProbePointsD3CN(tri.b2c[k], p);
            FOR s := 0 TO LAST(p) DO f[s] := func.eval(p[s]) END;
            NHInterpolateTriangD3CN(tri.c2b[k], p, f, c0, c1)
          END
        END
      END;
      RETURN spline
    END
  END NHInterpolateD3CN;

PROCEDURE ComputeNHProbePointsD3CN(
    READONLY b2c: LR3x3.T;
    VAR (*OUT*) p: ARRAY OF LR3.T
  ) =
  (*
    Computes the probe points for H^3_{-1} + H^2_{-1} 
    interpolation in a spherical triangle, with corners 
    given by the matrix "b2c".
  *)
  CONST
    deg = 3;
    NC0 = (deg + 1) * (deg + 2) DIV 2;
    NC1 = deg * (deg + 1) DIV 2;
    NP = NC0 + NC1;
  VAR b: LR3.T; 
  VAR s: CARDINAL := 0;
  CONST
    alpha1 = 0.9619397663d0;
    beta1  = 0.0190301169d0;
    gamma1 = 0.0190301169d0;
  CONST
    alpha2 = 0.6913417163d0;
    beta2  = 0.2634563246d0;
    gamma2 = 0.0452019592d0;
  CONST
    alpha3 = 0.9330127020d0;
    beta3  = 0.0334936490d0;
    gamma3 = 0.0334936490d0;
  CONST
    alpha4 = 0.4665063510d0;
    beta4  = 0.4665063510d0;
    gamma4 = 0.0669872980d0;
  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;
    (* Sample point at center of face: *)
    b := LR3.T{1.0d0, 1.0d0, 1.0d0};
    p[s] := LR3.Dir(LR3x3.MapCol(b2c, b)); INC(s);
    <* ASSERT s = NP *>
  END ComputeNHProbePointsD3CN;
  
PROCEDURE NHInterpolateTriangD3CN(
    READONLY c2b: LR3x3.T;
    READONLY p: ARRAY OF LR3.T;
    READONLY f: ARRAY OF LONG;
    (* OUT: *)
    VAR c0: ARRAY OF LONG;
    VAR c1: ARRAY OF LONG;
  ) =
  CONST
    deg = 3;
    NC0 = (deg + 1) * (deg + 2) DIV 2;
    NC1 = deg * (deg + 1) DIV 2;
    NP = NC0 + NC1;
  VAR 
    A: ARRAY [0..NP-1] OF ARRAY [0..NC0+NC1] OF LONGREAL;
    x: ARRAY [0..NC0+NC1-1] OF LONGREAL;
  BEGIN
    <* ASSERT NUMBER(p) = NP *>
    <* ASSERT NUMBER(f) = NP *>
    <* ASSERT NUMBER(c0) = NC0 *>
    <* ASSERT NUMBER(c1) = NC1 *>
    FOR s := 0 TO LAST(p) DO
      WITH
        bpf = LR3x3.MapCol(c2b, p[s])
      DO
        FOR ijk := 0 TO NC0-1 DO
          WITH lbl = IndexToLabel(ijk, deg) DO
            A[s,ijk] := BezierValueDim2(lbl[0], lbl[1], lbl[2], bpf)
          END
        END;
        FOR ijk := 0 TO NC1-1 DO
          WITH lbl = IndexToLabel(ijk, deg-1) DO
            A[s,ijk + NC0] := BezierValueDim2(lbl[0], lbl[1], lbl[2], bpf)
          END
        END;
        (* Independent term: *)
        A[s,NC0+NC1] := f[s]
      END
    END;

    (* Solve system for the coefficients associated with edge "ie" *)
    (* rintSystem("coefs", A); *)
    Triangularize(A);
    BackSolve(A, x);
    (* rintSolution("coefs", x); *)
    FOR ijk := 0 TO NC0-1 DO
      c0[ijk] := x[ijk];
    END;
    FOR ijk := 0 TO NC1-1 DO
      c1[ijk] := x[ijk + NC0]
    END;
    (* PrintCoeffs(c0, 3); *)
    (* PrintCoeffs(c1, 2); *)
  END NHInterpolateTriangD3CN;

PROCEDURE NHInterpolateD3C0(
    func: SPFunction.T;
    tri: REF SPTriang.T;
  ): SPBezPolyPWFunction.T =
  VAR lf: LR3.T;  (* Spherical laplacian of "f" at corners. *)
  BEGIN
    WITH 
      deg = 3,
      NT = NUMBER(tri.side^),
      NC0 = (deg + 1) * (deg + 2) DIV 2,
      NC1 = deg * (deg + 1) DIV 2,
      NP = 13, (* Number of probe points per triangle *)
      (* The function: *)
      spline = NEW(SPBezPolyPWFunction.T),
      (* Work vectors: *)
      p = NEW(REF ARRAY OF LR3.T, NP)^,
      f = NEW(REF ARRAY OF LONG, NP)^
    DO
     spline.tri:= tri;
     spline.data := MakeNHTriangles(NT, NC0, NC1);
     FOR k := 0 TO NT-1 DO
        WITH
          t = NARROW(spline.data[k], TriangleData)
        DO
          t.face := k;
          t.deg := deg;
          WITH
            c0 = t.c0^, c1 = t.c1^
          DO
            (* Wr.PutText(stderr, "triangle " & FI(k) & "\n"); *)
            ComputeNHProbePointsD3C0(tri.b2c[k], p);
            FOR s := 0 TO LAST(p) DO f[s] := func.eval(p[s]) END;
            FOR i := 0 TO 2 DO lf[i] := func.slap(p[i]) END;
            NHInterpolateTriangD3C0(tri.c2b[k], p, f, lf, c0, c1)
          END
        END
      END;
      RETURN spline
    END
  END NHInterpolateD3C0;
  
PROCEDURE ComputeNHProbePointsD3C0(
    READONLY b2c: LR3x3.T;
    VAR (*OUT*) p: ARRAY OF LR3.T
  ) =
  (*
    Computes the probe points for H^3_0 + H^2_0 
    interpolation in a spherical triangle,
    with corners given by the matrix "b2c".
  *)
  VAR b: LR3.T; 
  VAR s: CARDINAL := 0;
  CONST NP = 13;
  CONST
    alpha = 0.25d0;
    beta =  0.75d0;
  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 at center of face: *)
    b := LR3.T{1.0d0, 1.0d0, 1.0d0};
    p[s] := LR3.Dir(LR3x3.MapCol(b2c, b)); INC(s);
    <* ASSERT s = NP *>
  END ComputeNHProbePointsD3C0;
  
PROCEDURE NHInterpolateTriangD3C0(
    READONLY c2b: LR3x3.T;
    READONLY p: ARRAY OF LR3.T;
    READONLY f: ARRAY OF LONG;
    READONLY lf: LR3.T; (* Laplacian at corners. *)
    (* OUT: *)
    VAR c0: ARRAY OF LONG;
    VAR c1: ARRAY OF LONG;
  ) =
  VAR A: LR3x3.T; b: LR3.T;
  BEGIN
    FOR ijk := 0 TO LAST(c0) DO c0[ijk] := 0.0d0 END;
    FOR ijk := 0 TO LAST(c1) DO c1[ijk] := 0.0d0 END;
    (* Compute the vertex-associated coefficients: *)
    FOR i := 0 TO 2 DO 
      c0[CoeffIndex(3,0,0, i)] := 0.0d0;
      c1[CoeffIndex(2,0,0, i)] := f[i];
    END;
    (* 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(2,1, a[je], a[ke]);
            A[sx,1] := BezierValueDim1(1,2, a[je], a[ke]);
            A[sx,2] := BezierValueDim1(1,1, a[je], a[ke]);
            b[sx] := f[s] - SPDeCasteljau.Eval3(c0, a) - SPDeCasteljau.Eval2(c1, a)
          END
        END;
        WITH
          x = LR3x3.MapCol(LR3x3.Inv(A), b)
        DO
          c0[CoeffIndex(0,2,1, ie)] := x[0];
          c0[CoeffIndex(0,1,2, ie)] := x[1];
          c1[CoeffIndex(0,1,1, ie)] := x[2]
        END
      END
    END;
    
    (* Compute the face-associated coefficient: *)
    WITH
      a = LR3x3.MapCol(c2b, p[12]),
      A = BezierValueDim2(1,1,1, a),
      b = f[12] - SPDeCasteljau.Eval3(c0, a) - SPDeCasteljau.Eval2(c1, a)
    DO
      c0[4] := b/A
    END;
    (* PrintCoeffs(c0, 3); *)
    (* PrintCoeffs(c1, 2); *)
  END NHInterpolateTriangD3C0;
  
PROCEDURE NHInterpolateD6C1(
    func: SPFunction.T;
    tri: REF SPTriang.T;
  ): SPBezPolyPWFunction.T =
  BEGIN
    WITH 
      deg = 6,
      NT = NUMBER(tri.side^),
      NC0 = (deg + 1) * (deg + 2) DIV 2,
      NC1 = deg * (deg + 1) DIV 2,
      NF = 19, (* Number of probe points per triangle. *)
      NDL = 12,  (* Number of longitudinal derivatives. *)
      NDT = 15,  (* Number of transversal derivatives. *)
      (* The function: *)
      spline = NEW(SPBezPolyPWFunction.T),
      (* Work vectors: *)
      pf = NEW(REF ARRAY OF LR3.T, NF)^,   (* Probe points for function values. *)
      pdl = NEW(REF ARRAY OF LR3.T, NDL)^, (* Probe points for long. deriv. *)
      pdt = NEW(REF ARRAY OF LR3.T, NDT)^, (* Probe points for trans. deriv. *)
      
      gdl = NEW(REF ARRAY OF LR3.T, NDL)^, (* Directions of long. derivs. *)
      gdt = NEW(REF ARRAY OF LR3.T, 3)^,   (* Directions of trans. derivs. *)
      
      f = NEW(REF ARRAY OF LONG, NF)^,     (* Function values. *)
      dl = NEW(REF ARRAY OF LONG, NDL)^,   (* Longitudinal derivatives. *)
      dt = NEW(REF ARRAY OF LONG, NDT)^    (* Transversal derivatives. *)
      (*
        "f[s]" is the function value at "pf[s]", "s IN [0..NF-1]".
        
        "dl[s]" is the longitudinal derivative at point
          "pdl[s]" IN direction "gdl[s]", for "s IN [0..NDL-1]" .
        
        "dt[s]" is the transversal derivative
          at "pdt[s]";  "gdt[ie]" is the direction
          orthogonal to the edge "ie" opposite
          to vertex "ie".
      *)
    DO
     spline.tri:= tri;
     spline.data := MakeNHTriangles(NT, NC0, NC1);
     FOR k := 0 TO NT-1 DO
        WITH
          t = NARROW(spline.data[k], TriangleData)
        DO
          t.face := k;
          t.deg := deg;
          WITH c0 = t.c0^, c1 = t.c1^ DO
            (* Wr.PutText(stderr, "triangle " & FI(k) & "\n"); *)
            ComputeNHProbePointsD6C1(tri.b2c[k], pf, pdl, gdl, pdt, gdt);
            (* Compute function's values and derivatives: *)
            FOR s := 0 TO LAST(pf) DO f[s] := func.eval(pf[s]) END;
            FOR s := 0 TO LAST(pdl) DO 
              dl[s] := LR3.Dot(func.grad(pdl[s]), gdl[s])
            END;
            FOR ie := 0 TO 2 DO
              FOR sx := 0 TO 4 DO
                WITH s = 5*ie + sx DO
                  dt[s] := LR3.Dot(func.grad(pdt[s]), gdt[ie])
                END;
              END;
            END;
            (* Compute Bezier coefficients OF interpolating poly: *)
            NHInterpolateTriangD6C1(tri.c2b[k], 
              pf, f, pdl, gdl, dl, pdt, gdt, dt, c0, c1
            );
            (* Consistency check *)
            NHCheckTriangD6C1(tri.c2b[k], 
              pf, f, pdl, gdl, dl, pdt, gdt, dt, c0, c1
            )
          END
        END
      END;
      RETURN spline
    END
  END NHInterpolateD6C1;
  
PROCEDURE ComputeNHProbePointsD6C1(
    READONLY b2c: LR3x3.T;
    VAR (*OUT*) pf: ARRAY OF LR3.T;
    VAR (*OUT*) pdl: ARRAY OF LR3.T;
    VAR (*OUT*) gdl: ARRAY OF LR3.T;
    VAR (*OUT*) pdt: ARRAY OF LR3.T;
    VAR (*OUT*) gdt: ARRAY OF LR3.T;
  ) =
  (*
    Computes the probe points and directions for H^6_1 + H^5_1 
    interpolation in a spherical triangle,
    with corners given by the matrix "b2c".
  *)
  VAR b: LR3.T; 
  VAR s: CARDINAL := 0;
  CONST 
    NF = 19;
    NDL = 12;
    NDT = 15;
  CONST
    alpha1 = 5.0d0/6.0d0;
    beta1 =  1.0d0 - alpha1;
  CONST
    alpha2 = 2.0d0/3.0d0;
    beta2 =  1.0d0 - alpha2;
  CONST
    sigma1 = 11.0d0/12.0d0;
    tau1 =  1.0d0 - sigma1;
  CONST
    sigma2 = 9.0d0/12.0d0;
    tau2 =  1.0d0 - sigma2;
  BEGIN
    (* FUNCTION PROBE POINTS *)
    (* Sample points at corners: *)
    FOR i := 0 TO 2 DO
      b := LR3.T{0.0d0, 0.0d0, 0.0d0};
      b[i] := 1.0d0;
      pf[s] := LR3.Dir(LR3x3.MapCol(b2c, b)); INC(s);
    END;
    (* Sample points along edges: *)
    FOR ie := 0 TO 2 DO
      WITH je = (ie+1) MOD 3, ke = (ie+2) MOD 3 DO
        b[ie] := 0.0d0; b[je] := alpha1; b[ke] := beta1;
        pf[s] := LR3.Dir(LR3x3.MapCol(b2c, b)); INC(s);

        b[ie] := 0.0d0; b[je] := alpha2; b[ke] := beta2;
        pf[s] := LR3.Dir(LR3x3.MapCol(b2c, b)); INC(s);

        b[ie] := 0.0d0; b[je] := 0.50d0; b[ke] := 0.50d0;
        pf[s] := LR3.Dir(LR3x3.MapCol(b2c, b)); INC(s);

        b[ie] := 0.0d0; b[je] := beta2; b[ke] := alpha2;
        pf[s] := LR3.Dir(LR3x3.MapCol(b2c, b)); INC(s);

        b[ie] := 0.0d0; b[je] := beta1; b[ke] := alpha1;
        pf[s] := LR3.Dir(LR3x3.MapCol(b2c, b)); INC(s);
      END
    END;
    (* Sample point at center of face: *)
    b := LR3.T{1.0d0, 1.0d0, 1.0d0};
    pf[s] := LR3.Dir(LR3x3.MapCol(b2c, b)); INC(s);
    <* ASSERT s = NF *>
    
    (* LONGITUDINAL DERIVATIVE PROBE POINTS *)
    s := 0;
    FOR ie := 0 TO 2 DO
      WITH
        je = (ie+1) MOD 3, ke = (ie+2) MOD 3,
        u = LR3.Sub(pf[ke],pf[je]),
        v = LR3.Sub(pf[je],pf[ke])
      DO
        (* Longitudinal derivatives: *)
        pdl[s] := pf[je];     gdl[s] := LR3.Dir(LR3.Orthize(u, pdl[s])); INC(s);
        pdl[s] := pf[5*ie+4]; gdl[s] := LR3.Dir(LR3.Orthize(v, pdl[s])); INC(s);
        pdl[s] := pf[5*ie+6]; gdl[s] := LR3.Dir(LR3.Orthize(u, pdl[s])); INC(s);
        pdl[s] := pf[ke];     gdl[s] := LR3.Dir(LR3.Orthize(v, pdl[s])); INC(s);
      END
    END;
    <* ASSERT s = NDL *>
    
    (* TRANSVERSAL DERIVATIVE PROBE POINTS *)
    s := 0;
    FOR ie := 0 TO 2 DO
      WITH
        je = (ie+1) MOD 3, ke = (ie+2) MOD 3,
        o = LR3.Dir(LR3Extras.Cross(pf[je], pf[ke]))
      DO
        gdt[ie] := o;

        b[ie] := 0.0d0; b[je] := sigma1; b[ke] := tau1;
        pdt[s] := LR3.Dir(LR3x3.MapCol(b2c, b)); INC(s);

        b[ie] := 0.0d0; b[je] := sigma2; b[ke] := tau2;
        pdt[s] := LR3.Dir(LR3x3.MapCol(b2c, b)); INC(s);

        b[ie] := 0.0d0; b[je] := 0.50d0; b[ke] := 0.50d0;
        pdt[s] := LR3.Dir(LR3x3.MapCol(b2c, b)); INC(s);

        b[ie] := 0.0d0; b[je] := tau2; b[ke] := sigma2;
        pdt[s] := LR3.Dir(LR3x3.MapCol(b2c, b)); INC(s);

        b[ie] := 0.0d0; b[je] := tau1; b[ke] := sigma1;
        pdt[s] := LR3.Dir(LR3x3.MapCol(b2c, b)); INC(s);
      END
    END;
    <* ASSERT s = NDT *>
  END ComputeNHProbePointsD6C1;
  
PROCEDURE NHInterpolateTriangD6C1(
    READONLY c2b: LR3x3.T;
    READONLY pf: ARRAY OF LR3.T;
    READONLY f: ARRAY OF LONG;
    READONLY pdl: ARRAY OF LR3.T;
    READONLY gdl: ARRAY OF LR3.T;
    READONLY dl: ARRAY OF LONG;
    READONLY pdt: ARRAY OF LR3.T;
    READONLY gdt: ARRAY OF LR3.T;
    READONLY dt: ARRAY OF LONG;
    (* OUT: *)
    VAR c0: ARRAY OF LONG;
    VAR c1: ARRAY OF LONG;
  ) =
  BEGIN
    FOR ijk := 0 TO LAST(c0) DO c0[ijk] := 0.0d0 END;
    FOR ijk := 0 TO LAST(c1) DO c1[ijk] := 0.0d0 END;
    
    (* VERTEX-ASSOCIATED COEFFICIENTS: *)
    (* From function values at corners, and magic split: *)
    FOR ie := 0 TO 2 DO
      c0[CoeffIndex(6,0,0, ie)] := 0.0d0;
      c1[CoeffIndex(5,0,0, ie)] := f[ie];
    END;
    
    (* EDGE-ASSOCIATED COEFFICIENTS: *)
    (* From function values and long. derivs. along edges. *)
    VAR 
      A: ARRAY [0..8] OF ARRAY [0..9] OF LONGREAL;
      x: ARRAY [0..8] OF LONGREAL;
    BEGIN
      FOR ie := 0 TO 2 DO
        WITH je = (ie+1) MOD 3, ke = (ie+2) MOD 3 DO
          (* Build linear system for edge "ie" *)
          (* Equations implied by given function values: *)
          FOR sx := 0 TO 4 DO
            WITH
              s = 5*ie + sx +3,
              bpf = LR3x3.MapCol(c2b, pf[s])
            DO
              A[sx,0] := BezierValueDim1(5,1, bpf[je], bpf[ke]);
              A[sx,1] := BezierValueDim1(4,2, bpf[je], bpf[ke]);
              A[sx,2] := BezierValueDim1(3,3, bpf[je], bpf[ke]);
              A[sx,3] := BezierValueDim1(2,4, bpf[je], bpf[ke]);
              A[sx,4] := BezierValueDim1(1,5, bpf[je], bpf[ke]);
              A[sx,5] := BezierValueDim1(4,1, bpf[je], bpf[ke]);
              A[sx,6] := BezierValueDim1(3,2, bpf[je], bpf[ke]);
              A[sx,7] := BezierValueDim1(2,3, bpf[je], bpf[ke]);
              A[sx,8] := BezierValueDim1(1,4, bpf[je], bpf[ke]);
              (* Independent term: *)
              A[sx,9] := f[s] - SPDeCasteljau.Eval6(c0, bpf) - SPDeCasteljau.Eval5(c1, bpf)
            END
          END;
          (* Equations implied by the longitudinal derivatives: *)
          FOR sx := 0 TO 3 DO
            WITH
              s = 4*ie + sx,
              bpdl = LR3x3.MapCol(c2b, pdl[s]),
              bgdl = LR3x3.MapCol(c2b, gdl[s])
            DO
              <* ASSERT ABS(bpdl[ie]) < 1.0d-8 *>
              <* ASSERT ABS(bgdl[ie]) < 1.0d-8 *>
              A[sx+5,0] := BezierDerivDim1(5,1, bpdl[je], bpdl[ke], bgdl[je], bgdl[ke]);
              A[sx+5,1] := BezierDerivDim1(4,2, bpdl[je], bpdl[ke], bgdl[je], bgdl[ke]);
              A[sx+5,2] := BezierDerivDim1(3,3, bpdl[je], bpdl[ke], bgdl[je], bgdl[ke]);
              A[sx+5,3] := BezierDerivDim1(2,4, bpdl[je], bpdl[ke], bgdl[je], bgdl[ke]);
              A[sx+5,4] := BezierDerivDim1(1,5, bpdl[je], bpdl[ke], bgdl[je], bgdl[ke]);
              A[sx+5,5] := BezierDerivDim1(4,1, bpdl[je], bpdl[ke], bgdl[je], bgdl[ke]);
              A[sx+5,6] := BezierDerivDim1(3,2, bpdl[je], bpdl[ke], bgdl[je], bgdl[ke]);
              A[sx+5,7] := BezierDerivDim1(2,3, bpdl[je], bpdl[ke], bgdl[je], bgdl[ke]);
              A[sx+5,8] := BezierDerivDim1(1,4, bpdl[je], bpdl[ke], bgdl[je], bgdl[ke]);
              (* Independent term: *)
              A[sx+5,9] := dl[s] 
                - LR3.Dot(LR3x3.MapRow(SPDeCasteljau.EvalGrad6(c0, bpdl), c2b), gdl[s])
                - LR3.Dot(LR3x3.MapRow(SPDeCasteljau.EvalGrad5(c1, bpdl), c2b), gdl[s])
            END
          END;

          (* Solve system for the coefficients associated with edge "ie" *)
          PrintSystem("edge " & FI(ie,1) & " coefs", A);
          Triangularize(A);
          BackSolve(A, x);
          PrintSolution("edge " & FI(ie,1) & " coefs", x);
          c0[CoeffIndex(0,5,1, ie)] := x[0];
          c0[CoeffIndex(0,4,2, ie)] := x[1];
          c0[CoeffIndex(0,3,3, ie)] := x[2];
          c0[CoeffIndex(0,2,4, ie)] := x[3];
          c0[CoeffIndex(0,1,5, ie)] := x[4];

          c1[CoeffIndex(0,4,1, ie)] := x[5];
          c1[CoeffIndex(0,3,2, ie)] := x[6];
          c1[CoeffIndex(0,2,3, ie)] := x[7];
          c1[CoeffIndex(0,1,4, ie)] := x[8]
        END
      END
    END;
    
    (* SECOND LAYER OF COEFFICIENTS: *)
    (* From transversal derivatives along edges. *)
    VAR 
      A: ARRAY [0..14] OF ARRAY [0..15] OF LONGREAL;
      x: ARRAY [0..14] OF LONGREAL;
    BEGIN
      (* Build linear system for all coeffs in second layer. *)
      (* Equations from edge 0 *)
      WITH bgdt = LR3x3.MapCol(c2b, gdt[0]) DO
        Wr.PutText(stderr, "transverse direction = " & FLR3(gdt[0],8,5) & " bar = " & FLR3(bgdt, 8,5) & "\n");
        FOR s := 0 TO 4 DO
          WITH bpdt = LR3x3.MapCol(c2b, pdt[s]) DO
            Wr.PutText(stderr, "sample point = " & FLR3(pdt[s],8,5) & " bar = " & FLR3(bpdt, 8,5) & "\n");
            FOR r := 0 TO 15 DO A[s,r] := 0.0d0 END;

            A[s,00] := BezierDerivDim2(1,4,1, bpdt, bgdt);
            A[s,01] := BezierDerivDim2(1,3,2, bpdt, bgdt);
            A[s,02] := BezierDerivDim2(1,2,3, bpdt, bgdt);
            A[s,05] := BezierDerivDim2(1,1,4, bpdt, bgdt);

            A[s,03] := BezierDerivDim2(1,3,1, bpdt, bgdt);
            A[s,04] := BezierDerivDim2(1,2,2, bpdt, bgdt);
            A[s,08] := BezierDerivDim2(1,1,3, bpdt, bgdt);

            (* Independent term: *)
            A[s,15] := dt[s] 
              - LR3.Dot(LR3x3.MapRow(SPDeCasteljau.EvalGrad6(c0, bpdt), c2b), gdt[0])
              - LR3.Dot(LR3x3.MapRow(SPDeCasteljau.EvalGrad5(c1, bpdt), c2b), gdt[0])
          END
        END;
      END;
      (* Equations from edge 1 *)
      WITH bgdt = LR3x3.MapCol(c2b, gdt[1]) DO
        Wr.PutText(stderr, "transverse direction = " & FLR3(gdt[1],8,5) & " bar = " & FLR3(bgdt, 8,5) & "\n");
        FOR s := 5 TO 9 DO
          WITH bpdt = LR3x3.MapCol(c2b, pdt[s]) DO
            Wr.PutText(stderr, "sample point = " & FLR3(pdt[s],8,5) & " bar = " & FLR3(bpdt, 8,5) & "\n");
            FOR r := 0 TO 15 DO A[s,r] := 0.0d0 END;

            A[s,05] := BezierDerivDim2(1,1,4, bpdt, bgdt);
            A[s,06] := BezierDerivDim2(2,1,3, bpdt, bgdt);
            A[s,07] := BezierDerivDim2(3,1,2, bpdt, bgdt);
            A[s,10] := BezierDerivDim2(4,1,1, bpdt, bgdt);

            A[s,08] := BezierDerivDim2(1,1,3, bpdt, bgdt);
            A[s,09] := BezierDerivDim2(2,1,2, bpdt, bgdt);
            A[s,13] := BezierDerivDim2(3,1,1, bpdt, bgdt);

            (* Independent term: *)
            A[s,15] := dt[s] 
              - LR3.Dot(LR3x3.MapRow(SPDeCasteljau.EvalGrad6(c0, bpdt), c2b), gdt[1])
              - LR3.Dot(LR3x3.MapRow(SPDeCasteljau.EvalGrad5(c1, bpdt), c2b), gdt[1])
          END
        END;
      END;
      (* Equations from edge 2 *)
      WITH bgdt = LR3x3.MapCol(c2b, gdt[2]) DO
        Wr.PutText(stderr, "transverse direction = " & FLR3(gdt[2],8,5) & " bar = " & FLR3(bgdt, 8,5) & "\n");
        FOR s := 10 TO 14 DO
          WITH bpdt = LR3x3.MapCol(c2b, pdt[s]) DO
            Wr.PutText(stderr, "sample point = " & FLR3(pdt[s],8,5) & " bar = " & FLR3(bpdt, 8,5) & "\n");
            FOR r := 0 TO 15 DO A[s,r] := 0.0d0 END;

            A[s,10] := BezierDerivDim2(4,1,1, bpdt, bgdt);
            A[s,11] := BezierDerivDim2(3,2,1, bpdt, bgdt);
            A[s,12] := BezierDerivDim2(2,3,1, bpdt, bgdt);
            A[s,00] := BezierDerivDim2(1,4,1, bpdt, bgdt);

            A[s,13] := BezierDerivDim2(3,1,1, bpdt, bgdt);
            A[s,14] := BezierDerivDim2(2,2,1, bpdt, bgdt);
            A[s,03] := BezierDerivDim2(1,3,1, bpdt, bgdt);

            (* Independent term: *)
            A[s,15] := dt[s] 
              - LR3.Dot(LR3x3.MapRow(SPDeCasteljau.EvalGrad6(c0, bpdt), c2b), gdt[2])
              - LR3.Dot(LR3x3.MapRow(SPDeCasteljau.EvalGrad5(c1, bpdt), c2b), gdt[2])
          END
        END;
      END;
      (* Solve system for the coefficients in the second layer *)
      PrintSystem("second layer", A);
      Triangularize(A);
      PrintSystem("triangularized system", A);
      BackSolve(A, x);
      PrintSolution("second layer", x);
      FOR ie := 0 TO 2 DO
        c0[CoeffIndex(1,4,1, ie)] := x[5*ie+0];
        c0[CoeffIndex(1,3,2, ie)] := x[5*ie+1];
        c0[CoeffIndex(1,2,3, ie)] := x[5*ie+2];

        c1[CoeffIndex(1,3,1, ie)] := x[5*ie+3];
        c1[CoeffIndex(1,2,2, ie)] := x[5*ie+4];
      END
    END;
    
    (* CENTRAL COEFFICIENT: *)
    WITH
      a = LR3x3.MapCol(c2b, pf[18]),
      A = BezierValueDim2(2,2,2, a),
      b = f[18] - SPDeCasteljau.Eval6(c0, a) - SPDeCasteljau.Eval5(c1, a)
    DO
      c0[12] := b/A
    END;
    (* PrintCoeffs(c0, 3); *)
    (* PrintCoeffs(c1, 2); *)
  END NHInterpolateTriangD6C1;
  
PROCEDURE NHCheckTriangD6C1(
    READONLY c2b: LR3x3.T;
    READONLY pf: ARRAY OF LR3.T;
    READONLY f: ARRAY OF LONG;
    READONLY pdl: ARRAY OF LR3.T;
    READONLY gdl: ARRAY OF LR3.T;
    READONLY dl: ARRAY OF LONG;
    READONLY pdt: ARRAY OF LR3.T;
    READONLY gdt: ARRAY OF LR3.T;
    READONLY dt: ARRAY OF LONG;
    (* OUT: *)
    VAR c0: ARRAY OF LONG;
    VAR c1: ARRAY OF LONG;
  ) =
  BEGIN
    Wr.PutText(stderr, "checking coeffs...\n");
    (* Check function values: *)
    FOR s := 0 TO LAST(pf) DO
      WITH
        bpf = LR3x3.MapCol(c2b, pf[s]),
        bf = SPDeCasteljau.Eval6(c0, bpf)
           + SPDeCasteljau.Eval5(c1, bpf)
      DO
        IF ABS(bf - f[s]) > 1.0d-8 THEN
          Wr.PutText(stderr, "f(pf[" & FI(s) & "]) = " & FLRFix(bf, 8,5) & " should be " & FLRFix(f[s],8,5) & "\n");
        END;
      END;
    END;
    (* Check longitudinal derivatives: *)
    FOR s := 0 TO LAST(pdl) DO
      WITH
        bpdl = LR3x3.MapCol(c2b, pdl[s]),
        bdl = LR3.Dot(LR3x3.MapRow(SPDeCasteljau.EvalGrad6(c0, bpdl), c2b), gdl[s])
            + LR3.Dot(LR3x3.MapRow(SPDeCasteljau.EvalGrad5(c1, bpdl), c2b), gdl[s])
      DO
        IF ABS(bdl - dl[s]) > 1.0d-8 THEN
          Wr.PutText(stderr, "dl(pdl[" & FI(s) & "]) = " & FLRFix(bdl, 8,5) & " should be " & FLRFix(dl[s],8,5) & "\n");
        END;
      END;
    END;
    (* Check transversal derivatives: *)
    FOR ie := 0 TO 2 DO
      FOR s := 5*ie TO 5*ie+4 DO
        WITH
          bpdt = LR3x3.MapCol(c2b, pdt[s]),
          bdt = LR3.Dot(LR3x3.MapRow(SPDeCasteljau.EvalGrad6(c0, bpdt), c2b), gdt[ie])
              + LR3.Dot(LR3x3.MapRow(SPDeCasteljau.EvalGrad5(c1, bpdt), c2b), gdt[ie])
        DO
          IF ABS(bdt - dt[s]) > 1.0d-8 THEN
            Wr.PutText(stderr, "dt(pdt[" & FI(s) & "]) = " & FLRFix(bdt, 8,5) & " should be " & FLRFix(dt[s],8,5) & "\n");
          END;
        END;
      END
    END;
  END NHCheckTriangD6C1;

PROCEDURE PrintSystem(
    msg: TEXT;
    READONLY A: ARRAY OF ARRAY OF LONG;
  ) =
  BEGIN
    WITH N = NUMBER(A) DO
      <* ASSERT NUMBER(A[0]) = N + 1 *>
      Wr.PutText(stderr, "linear system - " & msg & "\n");
      FOR i := 0 TO N-1 DO
        Wr.PutText(stderr, FI(i, 2) & " ");
        FOR j := 0 TO N-1 DO
          Wr.PutText(stderr, FLRFix(A[i,j], 5,2) & " ");
        END;
        Wr.PutText(stderr, " = ");
        Wr.PutText(stderr, FLRFix(A[i,N], 8,5) & "\n");
      END;
      Wr.PutText(stderr, "\n");
    END
  END PrintSystem;
  
PROCEDURE PrintSolution(
    msg: TEXT;
    READONLY x: ARRAY OF LONG;
  ) =
  BEGIN
    Wr.PutText(stderr, "computed coefficients - " & msg & "\n");
    FOR j := 0 TO LAST(x) DO
      Wr.PutText(stderr, FLR(x[j], 8,5) & " ");
    END;
    Wr.PutText(stderr, "\n\n");
  END PrintSolution;
  
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;

<*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 BezierDerivDim1(i, j: CARDINAL; px, py: LONG; gx, gy: LONG): LONG =
  (*
    Derivative of univariate Bezier polynomial "B^{i+j}_{i,j}"
    at argument "(px,py)" for the differential "(gx,gy)"
    (both in barycentric coordinates). *)
  BEGIN
    RETURN FLOAT(i+j, LONG) 
      * ( gx * BezierValueDim1(i-1,j, px, py)
        + gy * BezierValueDim1(i,j-1, px, py)
        )
  END BezierDerivDim1;
  
PROCEDURE BezierDerivDim2(i,j,k: CARDINAL; READONLY p, g: LR3.T): LONG =
  (*
    Derivative of Bezier polynomial "B^{i+j+k}_{i,j,k}"
    at point "p" along direction "g"
    (both in barycentric cordinates). *)
  BEGIN
    RETURN FLOAT(i+j+k, LONG) 
      * ( g[0] * BezierValueDim2(i-1,j,k, p)
        + g[1] * BezierValueDim2(i,j-1,k, p)
        + g[2] * BezierValueDim2(i,j,k-1, p)
        )
  END BezierDerivDim2;

PROCEDURE MakeNHTriangles(
    n: CARDINAL; (* Number of triangles *)
    NC0, NC1: CARDINAL; 
  ): REF Triangles =
  (*
    Allocates an array of "n" triangle coefficient vectors 
    for a non-homogeneous spherical spline of degree "deg".
    Assumes that the homogeneous components of degree "deg"
    and "deg-1" have "NC0" and "NC1" coefficients per triangle,
    respectively. *)
  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.c0 := NEW(REF ControlValues, NC0);
          ti.c1 := NEW(REF ControlValues, NC1);
          WITH c0 = ti.c0^, c1 = ti.c1^ DO 
            FOR i := 0 TO NC0-1 DO c0[i] := 0.0d0 END;
            FOR i := 0 TO NC1-1 DO c1[i] := 0.0d0 END
          END;
        END
      END;
      RETURN r
    END
  END MakeNHTriangles;

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 FLR3(x: LR3.T; wid, prec: CARDINAL): TEXT =
  BEGIN
    RETURN 
      "( " &
      FLR(x[0], wid, prec) & ", " &
      FLR(x[1], wid, prec) & ", " &
      FLR(x[2], wid, prec) & " )"
  END FLR3;
  
PROCEDURE FLRFix(x: LONGREAL; wid, prec: CARDINAL): TEXT =
  BEGIN
    RETURN Fmt.Pad(Fmt.LongReal(x, style:= Fmt.Style.Fix, prec:= prec), wid)
  END FLRFix;
  
PROCEDURE FI(x: INTEGER; wid: CARDINAL := 0): TEXT =
  BEGIN
    RETURN Fmt.Pad(Fmt.Int(x), wid)
  END FI;

BEGIN
END SPNHInterpolate.