MODULE Curvature2D;

IMPORT Triangulation, LR4, Stat, Octf;

FROM Triangulation IMPORT Topology, Face, OrgV;
FROM Energy IMPORT Coords, Gradient;
FROM LR4 IMPORT Add, Neg, Dot;
FROM Octf IMPORT Enext, Enext_1;

CONST
  zero = LR4.T{0.0d0,0.0d0,0.0d0,0.0d0};
  IniStackSize = 100000;
  Epsilon = 0.0000000001d0;

VAR str,stc: Stat.T;        (* statistical accumulators to the number of "root"
                               elements and the number of "children" elements
                               inside each "root" element.
		            *) 			    
TYPE
  BOOLS = ARRAY OF BOOLEAN;
  StackF = REF ARRAY OF Face;

  Number = RECORD
    nre : INTEGER;     (* number of "root" faces. *)
    nce : CARDINAL;    (* number of "children" faces inside each "root" face.*)
  END;
   
REVEAL
  T = Public BRANDED OBJECT
      K   : LONGREAL;                (* The energy normalization factor      *)
      top : Topology;                (* The topology                         *)
      vVar: REF BOOLS;               (* TRUE if vertex is variable           *)
      num : Number;                  (* Number of "root" and "children" faces*)
      ChilFace: REF ARRAY OF StackF; (* The "children" faces                 *)
    OVERRIDES
      init := Init;
      defTop := DefTop;
      defVar := DefVar;
      eval := Eval;
      name := Name;
    END;

PROCEDURE Statistics(READONLY top: Topology) : Number =
  VAR num: Number;
  BEGIN
    FOR i:= 0 TO top.NF-1 DO
      WITH
        f   = top.face[i],
        fr  = FLOAT(f.root,REAL)
      DO
        Stat.Accum(str,fr);
	IF fr = 0.0 THEN Stat.Accum(stc,fr) END;
      END
    END;  
    num.nre :=  FLOOR(str.maximum)+1;
    num.nce :=  FLOOR(stc.num);
    RETURN num;  	
  END Statistics;

PROCEDURE CropChilFaces(
  READONLY top: Triangulation.Topology;
  READONLY num: Number;
  ) : REF ARRAY OF StackF =
  VAR topi : REF ARRAY OF CARDINAL;
  (* Crop the "children" faces for each "root" face. *)
  BEGIN
    (* initialize the "top" indexes for each of the "num.nre"
       stacks of faces.
    *)
    topi := NEW(REF ARRAY OF CARDINAL, num.nre);
    FOR k := 0 TO num.nre-1 DO topi[k] := 0 END;

    WITH    
      t = NEW(REF ARRAY OF StackF, num.nre)
    DO
      FOR k := 0 TO num.nre-1 DO
        t[k] := NEW(REF ARRAY OF Face, IniStackSize);
      END;

      FOR j := 0 TO top.NF-1 DO
	WITH
	  f = top.face[j],
	  fr = f.root
	DO
          IF fr # -1 THEN SaveF(t[fr],topi[fr],f) END;
	END
      END;
      RETURN t;
    END;
  END CropChilFaces;

PROCEDURE SaveF(
   VAR Stack : StackF;
   VAR top: CARDINAL;
   VAR face  : Face;
  ) =
  (* Save the face "face" on the stack "Stack" *)
  BEGIN
    Stack[top] := face;
    top := top +1
  END SaveF;

PROCEDURE Init(erg: T) : T =
  BEGIN
    RETURN erg
  END Init;

PROCEDURE DefTop(erg: T; READONLY top: Topology) =
  BEGIN
    erg.K    := 1.0d0;      
    erg.top  := top;
    erg.vVar := NEW(REF BOOLS, top.NV);
    erg.num  := Statistics(top);
    erg.ChilFace := CropChilFaces(top,erg.num);
    
    (* Just in case the client forgets to call "defVar": *)
    FOR i := 0 TO top.NV-1 DO erg.vVar[i] := FALSE END;
  END DefTop;
  
PROCEDURE DefVar(erg: T; READONLY variable: BOOLS) =
  BEGIN
    WITH
      NV = erg.top.NV,
      vVar = erg.vVar^
    DO
      <* ASSERT NUMBER(variable) = NV *>
      vVar := variable;
    END
  END DefVar;

PROCEDURE Eval(
    erg: T; 
    READONLY c: Coords; 
    VAR e: LONGREAL; 
    grad: BOOLEAN;
    VAR eDc: Gradient;
  ) =
  BEGIN
    WITH
      NV   = erg.top.NV,
      K    = erg.K,
      vVar = erg.vVar^,
      ChilFace = erg.ChilFace,
      num = erg.num
    DO    
      PROCEDURE AddTerm(READONLY iu,iv,iw,ix: CARDINAL) =
        (*
          Adds one term of the curvature energy to "e" (and its
          derivative to "eDc, if "grad" is TRUE). The terms correspond
	  to the faces "f1 = u v w" and "f2 = u w x". See the follow
          picture:
	                       v
			      /|\
			     / | \    
			    /  |  \
            /	           /   |   \          \
	    ------	w /    |    \ x   -----
            \		  \ f1 | f2 /         /
			   \   |   /
			    \  |  /
			     \ | /
			      \|/
                               u

        *)
	VAR
	  eterm: LONGREAL;
          eDdu, eDdv, eDdw, eDdx: LR4.T;
        BEGIN
          WITH
            u = c[iu],
            v = c[iv],
            w = c[iw],
            x = c[ix]
          DO
            term(u,v,w,x, eterm, eDdu, eDdv, eDdw, eDdx);
            e := e + eterm;
            IF grad THEN
              IF vVar[iu] THEN
                eDc[iu] := LR4.Add(eDc[iu],eDdu);
              END;
	      IF vVar[iv] THEN
		eDc[iv] := LR4.Add(eDc[iv],eDdv);
	      END;
	      IF vVar[iw] THEN
		eDc[iw] := LR4.Add(eDc[iw],eDdw);
	      END;
              IF vVar[ix] THEN
		eDc[ix] := LR4.Add(eDc[ix],eDdx);
	      END
            END
          END
        END AddTerm;

     PROCEDURE term(u,v,w,x: LR4.T;
                    VAR eterm: LONGREAL;
                    VAR dedu,dedv,dedw,dedx: LR4.T;
                ) =
       VAR dedDv,dedDw,dedDx: LR4.T;
       BEGIN
         WITH
           Dv = LR4.Sub(v,u),   (* V *)
           Dw = LR4.Sub(w,u),   (* V *)
           Dx = LR4.Sub(x,u)
         DO
           EangleAux(Dv,Dw, Dx, eterm, dedDv,dedDw,dedDx);
           dedv := dedDv;       (* V *)
           dedw := dedDw;       (* V *)
           dedx := dedDx;       (* V *)
           dedu := Neg(Add(Add(dedDv,dedDw),dedDx));
         END
       END term;


        PROCEDURE EangleAux(f,a,b: LR4.T;
                       VAR eterm: LONGREAL;
                       VAR dedf,deda,dedb: LR4.T;
                      ) =
        (* compute the derivative of the orthogonal vectors "f" and "g" where
           "f= s - Proj(s,r)" and "g = r".
        *)
        VAR dedr,deds: LR4.T;
        BEGIN
          WITH
            m  = LR4.Dot(f,f)+Epsilon,            (* S *)
            u  = LR4.Dot(f,a),                    (* S *)
            v  = LR4.Dot(f,b),                    (* S *)
            U  = u/m,                             (* S *)
            V  = v/m,                             (* S *)
            Uf = LR4.Scale(U,f),                  (* V *)
            Vf = LR4.Scale(V,f),                  (* V *)
            R = LR4.Sub(a,Uf),                    (* V *)
            S = LR4.Sub(b,Vf)                     (* V *)
          DO
            Eangle(R,S,eterm,dedr,deds);
            WITH
              dedV   = - Dot(deds, f),                 (* S *)
              dedU   = - Dot(dedr, f),                 (* S *) 
              dedu   = dedU/m,                         (* S *)
              dedv   = dedV/m,                         (* S *)
              dedm   = (-1.0d0/m) * (dedU*U + dedV*V), (* S *) 
              dedm2  = LR4.Scale(2.0d0,f),             (* S *)
              dedm2f = LR4.Scale(dedm, dedm2),         (* V *)
              dedua  = LR4.Scale(dedu,  a),            (* V *)
              dedvb  = LR4.Scale(dedv,  b),            (* V *)
              dedrU  = LR4.Neg(LR4.Scale(U, dedr)),    (* V *)
              dedsV  = LR4.Neg(LR4.Scale(V, deds)),    (* V *)
              t1     = LR4.Add(dedm2f,dedua),          (* V *)
              t2     = LR4.Add(t1, dedvb),             (* V *)
              t3     = LR4.Add(t2, dedrU),             (* V *)
              t4     = LR4.Add(t3, dedsV),             (* V *)
              deduf  = LR4.Scale(dedu, f),             (* V *)
              dedvf  = LR4.Scale(dedv, f),             (* V *)
              c1     = LR4.Add(deduf, dedr),           (* V *)
              d1     = LR4.Add(dedvf, deds)            (* V *)
            DO
              dedf  := t4;
              deda  := c1;
              dedb  := d1;
            END
          END
        END EangleAux;

       PROCEDURE Eangle(
          READONLY R,S: LR4.T;
          VAR E: LONGREAL;
          VAR EDR,EDS: LR4.T;
          ) =
          (* Given two vectors "R" and "S" compute the "cos" of the angle
             between the vectors, the curvature term and the derivatives of
             energy respect to the two vectors: eeDR and eeDS.
          *)
          BEGIN
            WITH
              m = LR4.Norm(R) + Epsilon,
	      n = LR4.Norm(S) + Epsilon,
	      o = LR4.Dot(R,S),
	      d = m*n,
	      q = o/d
            DO
	      IF d # 0.0d0 THEN
                E := K * (1.0d0 + q);
                IF grad THEN
                  WITH
                    eDq = 1.0d0 * K,
                    eDo = eDq / d,
                    eDd = - eDq * q / d,
                    eDm = eDd * n,
                    eDn = eDd * m
                  DO
                    EDR := LR4.Mix(eDo, S, eDm/m, R);
                    EDS := LR4.Mix(eDo, R, eDn/n, S);
                  END
	        END
	      END   
            END
          END Eangle;

      BEGIN
        (* Initialize *)
        FOR i := 0 TO NV-1 DO eDc[i] := zero END;
        
        (* Compute energy "e", and the gradient "eDr": *)
        e := 0.0d0;
        FOR i := 0 TO num.nre-1  DO
	  FOR j := 0 TO num.nce-1 DO
	    FOR k := j+1 TO num.nce-1 DO
              IF ChilFace[i,j].exists AND ChilFace[i,k].exists THEN
		IF Adjacent(ChilFace[i,j], ChilFace[i,k]) THEN
		  WITH
		    u1 = OrgV(ChilFace[i,j].pa).num,
		    v1 = OrgV(Enext  (ChilFace[i,j].pa)).num,
		    w1 = OrgV(Enext_1(ChilFace[i,j].pa)).num,
		    u2 = OrgV(ChilFace[i,k].pa).num,
		    v2 = OrgV(Enext  (ChilFace[i,k].pa)).num,
		    w2 = OrgV(Enext_1(ChilFace[i,k].pa)).num
		  DO
		    IF (v1 = v2) AND (u1 = w2) THEN
		      AddTerm(v1,u1,w1,u2); 
		    ELSIF (v1 = w2) AND (u1 = u2) THEN
		      AddTerm(v1,u1,w1,v2);
		    ELSIF (v1 = u2) AND (u1 = v2) THEN
		      AddTerm(v1,u1,w1,w2);
		    ELSIF (u1 = v2) AND (w1 = w2) THEN
		      AddTerm(u1,w1,v1,u2);
		    ELSIF (u1 = w2) AND (w1 = u2) THEN
		      AddTerm(u1,w1,v1,v2);
		    ELSIF (u1 = u2) AND (w1 = v2) THEN
		      AddTerm(u1,w1,v1,w2);
		    ELSIF (w1 = v2) AND (v1 = w2) THEN
		      AddTerm(w1,v1,u1,u2);
		    ELSIF (w1 = w2) AND (v1 = u2) THEN
		      AddTerm(w1,v1,u1,v2);
		    ELSIF (w1 = u2) AND (v1 = v2) THEN
		      AddTerm(w1,v1,u1,w2);
		    ELSIF (v1 = w2) AND (u1 = v2) THEN
		      AddTerm(v1,u1,w1,u2);
		    ELSIF (v1 = v2) AND (u1 = u2) THEN
		      AddTerm(v1,u1,w1,w2);
		    ELSIF (v1 = u2) AND (u1 = w2) THEN
		      AddTerm(v1,u1,w1,v2);
		    ELSIF (u1 = w2) AND (w1 = v2) THEN
		      AddTerm(u1,w1,v1,u2);
		    ELSIF (u1 = v2) AND (w1 = u2) THEN
		      AddTerm(u1,w1,v1,w2);
		    ELSIF (u1 = u2) AND (w1 = w2) THEN
		      AddTerm(u1,w1,v1,v2);
		    ELSIF (w1 = w2) AND (v1 = v2) THEN
		      AddTerm(w1,v1,u1,u2);
		    ELSIF (w1 = v2) AND (v1 = u2) THEN
		      AddTerm(w1,v1,u1,w2);
		    ELSIF (w1 = u2) AND (v1 = w2) THEN
		      AddTerm(w1,v1,u1,v2);
		    END
                  END
	        END
              END	  
	    END	
 	  END   
        END
      END
    END  
  END Eval;

PROCEDURE Adjacent(f1,f2: Face) : BOOLEAN =
  VAR count : INTEGER := 0;
  BEGIN
    WITH
      u1 = OrgV(f1.pa).num,
      v1 = OrgV(Enext  (f1.pa)).num,
      w1 = OrgV(Enext_1(f1.pa)).num,
      u2 = OrgV(f2.pa).num,
      v2 = OrgV(Enext  (f2.pa)).num,
      w2 = OrgV(Enext_1(f2.pa)).num
    DO
      IF u1 = u2 OR u1 = v2 OR u1 = w2 THEN count := count + 1; END;
      IF v1 = u2 OR v1 = v2 OR v1 = w2 THEN count := count + 1; END;
      IF w1 = u2 OR w1 = v2 OR w1 = w2 THEN count := count + 1; END;
      IF count = 2 THEN RETURN TRUE
        ELSE RETURN FALSE
      END
    END  
  END Adjacent;
  

PROCEDURE Name(<* UNUSED *> erg: T): TEXT =
  BEGIN
    RETURN 
      "Curv2D()"
  END Name;

BEGIN
END Curvature2D.