MODULE ModKamSpring;

(* Last Version: 18-11-2000 *)

IMPORT LR4, Triangulation, Fmt, Stdio, Wr, Thread, Octf;
  
FROM Triangulation IMPORT Topology, OrgV;
FROM Energy IMPORT Coords, Gradient;
FROM Stdio IMPORT stderr;
FROM Octf IMPORT Clock;

CONST inf = LAST(CARDINAL);

TYPE
  BOOLS = ARRAY OF BOOLEAN;
  LONGS = ARRAY OF LONGREAL;
  AdjacencyMatrix = ARRAY OF ARRAY OF CARDINAL;
 
REVEAL
  T = Public BRANDED OBJECT
      top: Topology;             (* The topology *)
      termVar: REF BOOLS;        (* TRUE if vertex is variable & existing *)
      m : REF AdjacencyMatrix;   (* Matrix of initial distances *)
      eDdif: REF ARRAY OF LONGS; (* (Work) Gradient of "e" rel. to "dif"  *)
      L : LONGREAL;              (* length of spring *)
                                 
    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) =
  VAR dmax : LONGREAL;
  BEGIN
    WITH
      NV = top.NV
    DO
      erg.m := MakeAdjacencyMatrix(top);
      (*PrintHalfMatrix(erg.m, NV);*)
      ShortestPath(erg.m, NV);
      (*PrintHalfMatrix(erg.m, NV);*)
      dmax :=  FLOAT(FindMaxDistance(erg.m, NV), LONGREAL);
      erg.L := FLOAT(erg.length,LONGREAL)/dmax;
      erg.top := top;
      erg.termVar := NEW(REF BOOLS, NV);
      erg.eDdif := NEW(REF ARRAY OF LONGS, NV, NV);
      (*PrintHalfMatrix(erg.k, NV);*)
      
      (* Just in case the client forgets to call "defVar": *)
      FOR i := 0 TO top.NV-1 DO erg.termVar^[i] := FALSE END;
    END
  END DefTop;
 
PROCEDURE DefVar(erg: T; READONLY variable: BOOLS) =
  BEGIN
    (*
      Decide which vertices are relevant to kamada energy. A vertex is relevant
      iff it is variable.
    *)
    WITH
      NV = erg.top.NV,
      termVar = erg.termVar^
    DO
      (* Find the relevant vertices: *)
      <* ASSERT NUMBER(variable) = NV *>
      FOR v := 0 TO NV-1 DO
        termVar[v] := variable[v];
      END
    END
  END DefVar;

PROCEDURE MakeAdjacencyMatrix(
  READONLY top : Triangulation.Topology;
  ) : REF AdjacencyMatrix =
  VAR
    m: REF AdjacencyMatrix;
  BEGIN
    m := NEW(REF ARRAY OF ARRAY OF CARDINAL, top.NV, top.NV);
    FOR i := 0 TO top.NV-1 DO
      FOR j := 0 TO top.NV-1 DO
        m[i,j] := inf;
      END;
      m[i,i] := 0;
    END;

    FOR i := 0 TO top.NE-1 DO      
      WITH
        a = top.edge[i].pa,
	i = OrgV(a).num,
	j = OrgV(Clock(a)).num
      DO
        m[i,j] := 1;
	m[j,i] := 1;
      END
    END;
    RETURN m;
  END MakeAdjacencyMatrix;

PROCEDURE FindMaxDistance(
   READONLY m: REF AdjacencyMatrix;
   n: INTEGER;
   ) : CARDINAL =
  VAR max := 0; 
  BEGIN
    FOR i := 0 TO n-1 DO
      FOR j := 0 TO i DO
        IF m[i,j] > max  THEN
          max := m[i,j];
        END
      END
    END;
    RETURN max;
  END FindMaxDistance;
  
PROCEDURE CalculateStrength(dist : CARDINAL) : LONGREAL =
  BEGIN
    IF dist = 0 THEN
      RETURN 0.0d0
    ELSE
      WITH
        d = FLOAT(dist, LONGREAL)
      DO
        RETURN 1.0d0/(d*d);
      END
    END
  END CalculateStrength;

PROCEDURE CalculateLength(dist : CARDINAL) : LONGREAL =
  BEGIN
    IF dist = 0 THEN
      RETURN 0.0d0
    ELSE
      WITH
        d = FLOAT(dist, LONGREAL)
      DO
        RETURN d;
      END
    END
  END CalculateLength;

<* UNUSED *>
PROCEDURE PrintAdjacencyMatrix(READONLY m : REF AdjacencyMatrix; n: INTEGER) =
  <* FATAL Wr.Failure, Thread.Alerted *>
  BEGIN
    FOR i := 0 TO n-1 DO
      FOR j := 0 TO n-1 DO
        IF m[i,j] = inf THEN
          Wr.PutText(stderr, "# ");
        ELSE
          Wr.PutText(stderr, Fmt.Int(m[i,j]) & " ");
        END;
       END;
       Wr.PutText(stderr, "\n");
    END;
  END PrintAdjacencyMatrix;

<* UNUSED *>
PROCEDURE PrintHalfMatrix(READONLY m : REF AdjacencyMatrix; n: INTEGER) =
  <* FATAL Wr.Failure, Thread.Alerted *>
  BEGIN
    FOR i := 0 TO n-1 DO
      FOR j := 0 TO i DO
        IF m[i,j] = inf THEN
          Wr.PutText(stderr, "# ");
        ELSE
          Wr.PutText(stderr, Fmt.Int(m[i,j]) & " ");
        END;
       END;
       Wr.PutText(stderr, "\n");
    END;
  END PrintHalfMatrix;

PROCEDURE ShortestPath(VAR m : REF AdjacencyMatrix; n : INTEGER) =
  VAR s : CARDINAL;     
  BEGIN
    FOR i := 0 TO n-1 DO
      FOR j := 0 TO n-1 DO
        IF m[j,i] < inf THEN
          FOR k := 0 TO n-1 DO
            IF m[i,k] < inf THEN
              s := m[j,i] + m[i,k];
              IF s < m[j,k] THEN m[j,k] := s END;
            END
          END
        END
      END
    END
   END ShortestPath;
    
PROCEDURE Eval(
    erg: T; 
    READONLY c: Coords; 
    VAR e: LONGREAL; 
    grad: BOOLEAN;
    VAR eDc: Gradient;
  ) =
  BEGIN
    WITH
      NV = erg.top.NV,
      eDdif = erg.eDdif^,
      termVar = erg.termVar,
      length   = erg.L,
      strength = FLOAT(erg.strength, LONGREAL),
      m = erg.m^
    DO
      PROCEDURE AccumTerm(READONLY u: CARDINAL) =
        (*
          Adds to "e" the energy term corresponding to a vertex "u".
	  Returns also the gradient "eDdif".
        *)
        CONST Epsilon = 1.0d-10; 
        BEGIN  
          WITH
	    cu = c[u]
	  DO    
	    FOR i := u+1 TO NV-1 DO  
	      WITH
	        cv  = c[i],
                n   = LR4.Sub(cu,cv),
                dif = LR4.Norm(n),
		d2  = dif * dif + Epsilon,
		duv = m[u,i],
		luv = length * CalculateLength(duv),
		kuv = strength * CalculateStrength(duv),
		l2  = luv * luv + Epsilon,
		d3  = d2 * dif  + Epsilon
              DO
                e := e + kuv * ( (d2/l2) + (l2/d2) - 2.0d0 );
                IF grad THEN
                  eDdif[u,i] := 2.0d0 * kuv * ( (dif/l2) - (l2/d3) );
		  eDdif[i,u] := eDdif[u,i];
                ELSE
                  eDdif[u,i] := 0.0d0;
		  eDdif[i,u] := eDdif[u,i];
                END
              END
	     END 		
          END
       END AccumTerm;

      PROCEDURE Distribute_eDdif(READONLY u: CARDINAL) =
        (* Distribute eDdif on endpoints "c[u]" and "c[v]" *)
        CONST Epsilon = 1.0d-10;
        BEGIN
	  WITH
	    cu = c[u]
	  DO  
	    FOR i := u+1 TO NV-1 DO  
	      WITH
	        ci     = c[i],
                n      = LR4.Sub(cu,ci),
                dif    = LR4.Norm(n)+Epsilon,
                eDi    = eDc[i],
                eDu    = eDc[u],
                difDcu = LR4.Scale(1.0d0/dif, n),
                difDci = LR4.Scale(-1.0d0/dif,n),
                eDcu   = LR4.Scale(eDdif[u,i], difDcu),
                eDci   = LR4.Scale(eDdif[u,i], difDci)
              DO
                IF termVar[i] THEN
                  eDi := LR4.Add(eDi, eDci);
                END;
                IF termVar[u] THEN
                  eDu := LR4.Add(eDu, eDcu);
                END
              END
	    END   
          END;
        END Distribute_eDdif;
      
      BEGIN
        FOR i := 0 TO NV-1 DO eDc[i]:=LR4.T{0.0d0, 0.0d0, 0.0d0, 0.0d0} END;
        e := 0.0d0;
        FOR l := 0 TO NV-1 DO
          IF termVar[l] THEN
              AccumTerm(l);
              IF grad THEN
                Distribute_eDdif(l);
              END
            END
          END
        END
      END
    END Eval;

PROCEDURE Name(erg: T): TEXT =
  BEGIN
    RETURN
      "ModKamada(ilenth := " & Fmt.Real(erg.length,Fmt.Style.Fix,prec := 3) &
      " istrength := " & Fmt.Real(erg.strength,Fmt.Style.Fix,prec:= 3) & ")";
  END Name;

  
BEGIN
END ModKamSpring.