MODULE Curvature2D;

IMPORT Triangulation, LR4, Octf;

FROM Triangulation IMPORT Topology, OrgV, Pair;
FROM Energy IMPORT Coords, Gradient;
FROM LR4 IMPORT Add, Neg, Dot;
FROM Octf IMPORT Fnext, Clock, Enext_1;

CONST
  zero = LR4.T{0.0d0,0.0d0,0.0d0,0.0d0};
  Epsilon = 1.0d-9;

TYPE
  BOOLS = ARRAY OF BOOLEAN;
   
REVEAL
  T = Public BRANDED OBJECT
      K   : LONGREAL;                (* The energy normalization factor      *)
      top : Topology;                (* The topology                         *)
      vVar: REF BOOLS;               (* TRUE if vertex is variable           *)
    OVERRIDES
      init := Init;
      defTop := DefTop;
      defVar := DefVar;
      eval := Eval;
      name := Name;
    END;


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);
    (* 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 EdgeBelongsToOriginalFace(a: Pair; READONLY top:Topology): BOOLEAN =
  (* 
    TRUE if the edge component of "a" belongs to a face of the 
    original map.
  *)
  VAR b: Pair := a;
  BEGIN
    IF top.edge[a.facetedge.edge.num].root # -1 THEN RETURN FALSE END;
    
    (* Check if any face incident to "a" belongs to a face of the original map. *)
    REPEAT
      IF top.face[b.facetedge.face.num].root # -1 THEN RETURN TRUE END;
      b := Fnext(b)
    UNTIL b = a;
    RETURN FALSE
  END EdgeBelongsToOriginalFace;
  
PROCEDURE PiecesOfOriginalFace(a: Pair; READONLY top:Topology): ARRAY [0..1] OF Pair =
  (*
    Assumes that the edge component of "a" belongs to a face of the 
    original map.  Returns the two new faces incident to "a" that were
    part of that old face.
  *)
  VAR fg: ARRAY [0..1] OF Pair; 
  BEGIN 
    (* Look for the first face: *)
    WHILE top.face[a.facetedge.face.num].root = -1 DO a := Fnext(a) END;
    fg[0] := a;
    (* Look for second face. *)
    a := Fnext(a); 
    WHILE top.face[a.facetedge.face.num].root = -1 DO a := Fnext(a) END;
    fg[1] := a;
    <* ASSERT fg[0] # fg[1] *>
    RETURN fg
  END PiecesOfOriginalFace;

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^,
      top  = erg.top
    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 top.NE-1 DO
          WITH a = top.edge[i].pa DO
            IF EdgeBelongsToOriginalFace(a,top) THEN
              WITH
                fg = PiecesOfOriginalFace(a,top),
                f = fg[0],
                g = fg[1],
                u =  OrgV(f).num,
                v =  OrgV(Clock(f)).num,
                w1 = OrgV(Enext_1(f)).num,
                w2 = OrgV(Enext_1(g)).num
              DO
                AddTerm(u, v, w1, w2)
              END
	    END	
 	  END   
        END
      END
    END  
  END Eval;

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

BEGIN
END Curvature2D.