MODULE Heuristics;

IMPORT Oct, Triang, LR3, LR3Extras, LR3x3, Random, Math;
FROM Oct IMPORT Sym, Onext, Oprev, Degree;
FROM Triang IMPORT Arc, Coords, LeftF, OrgV;
IMPORT Debug, LRN;
 
CONST debug = FALSE;

PROCEDURE Dir(v: LR3.T): LR3.T =
  (* Same as LR3.Dir(v), but returns random unit vector if "v" is zero. *)
  BEGIN
    WITH s = LR3.Norm(v) DO
      IF s = 0.0d0 THEN
        RETURN LR3.T{1.0d0, 0.0d0, 0.0d0}
      ELSE
        RETURN LR3.Scale(1.0d0/s, v)
      END
    END    
  END Dir;

PROCEDURE ChooseReferenceArc(
     a: Arc; 
     READONLY variable: BOOLS; 
     coins: Random.T
  ): Arc =
  (*
    If any of the edges out of "Org(a)" has a fixed tip, picks 
    one such edge, with probability proportional to the
    number of "Onext"s until the next fixed edge.
    Otherwise picks a random arc out of "Org(a)". 
  *)
  BEGIN
    WITH
      k = coins.integer(0, Degree(a)-1)
    DO
      FOR i := 0 TO k-1 DO 
        IF NOT variable[OrgV(Sym(a)).num] THEN RETURN a END;
        a := Oprev(a) 
      END;
      RETURN a
    END
  END ChooseReferenceArc;

PROCEDURE SpreadVertex(
    a: Arc; 
    VAR c: Coords; 
    READONLY variable: BOOLS; 
    coins: Random.T;
  ) =
  BEGIN
    a := ChooseReferenceArc(a, variable, coins);
    WITH
      u = OrgV(a).num,
      m = Degree(a),
      normal = Triang.VertexNormal(a, c),
      v0 = OrgV(Sym(a)).num,
      uv0 = LR3.Sub(c[v0], c[u]),
      y = Dir(LR3Extras.Cross(normal, uv0)),
      x = Dir(LR3Extras.Cross(y, normal)),
      angle = 2.0d0 * FLOAT(Math.Pi, LONGREAL)/FLOAT(m, LONGREAL)
    DO
      FOR i := 0 TO m-1 DO
        WITH 
          vx = OrgV(Sym(a)), (* DstV(a) *)
          v = vx.num
        DO
          IF variable[v] AND vx.exists THEN
            WITH
              uv = LR3.Sub(c[v], c[u]),
              uv_n = LR3.Scale(LR3.Dot(uv, normal), normal), 
              uv_t = LR3.Sub(uv, uv_n),
              r = LR3.Norm(uv_t),
              t = angle * FLOAT(i, LONGREAL),
              ct = Math.cos(t),
              st = Math.sin(t),
              xy = LR3.Add(LR3.Scale(r*ct, x), LR3.Scale(r*st, y)),
              uw = LR3.Add(uv_n, xy)
            DO
              (* Nudge vertex "v" towards "w" *)
              c[v] := LR3.Add(c[u], LR3.Mix(0.75d0, uv, 0.25d0, uw))
            END;
          END
        END;
        a := Onext(a);
      END
    END
  END SpreadVertex;
  
PROCEDURE FlattenVertex(
    a: Arc; 
    VAR c: Coords;
    READONLY variable: BOOLS; 
    <*UNUSED*> coins: Random.T;
  ) =

  VAR somaki: LONGREAL := 0.0d0;
      somakiti: LONGREAL := 0.0d0;
      N: CARDINAL := 0;

  PROCEDURE CalcTi(READONLY bar: LR3.T; e: Arc) =
    (* 
      Computes the pseudo-force "ti" and its weight "ki"
      and accumulates them in "somatiki", "somaki"
    *)
    BEGIN
      (* Check if hinge exists and has a spring: *)
      WITH
        b = Oprev(Sym(e)),
        be = NARROW(b.edge, Triang.Edge),
        bl = LeftF(b),
        br = LeftF(Sym(b))
      DO
        IF NOT (bl.exists AND br.exists AND be.spring) THEN RETURN END
      END;
      (* Do the computations: *)
      WITH
        f = Onext(e),
        p = OrgV(Sym(e)).num,
        q = OrgV(Sym(f)).num,
        w = OrgV(Sym(Onext(Onext(Sym(f))))).num,

        di = LR3.Sub(c[q], c[p]),
        ni = LR3Extras.Cross(di, LR3.Sub(bar, c[q])),
        mi = LR3Extras.Cross(LR3.Sub(c[w], c[p]), di),

        dimod = LR3.Norm(di),
        nimod = LR3.Norm(ni),
        mimod = LR3.Norm(mi),

        sinTheta = LR3x3.Det(LR3x3.T{ni, mi, di}) / (nimod*mimod*dimod),
        cosTheta = LR3.Cos(ni, mi),
        theta = Math.atan2(sinTheta, cosTheta),
        feta = ThetaFunc(theta),

        ki = dimod,
        ti = feta * nimod / dimod
      DO
        IF debug THEN
          Debug.LongReal("CalcTi: ni = ", ni);
          Debug.LongReal("        mi = ", mi);
          Debug.LongReal("        di = ", di);
          Debug.LongReal("        sin, cos = ", LRN.T{sinTheta, cosTheta});
          Debug.LongReal("        theta, feta = ", LRN.T{theta, feta});
          Debug.LongReal("        ki, ti = ", LRN.T{ki, ti});
        END;
        somaki := somaki + ki;
        somakiti := somakiti + ki * ti;
        N := N+1;
      END;
    END CalcTi;

  PROCEDURE ThetaFunc(Theta: LONGREAL): LONGREAL =
    CONST PPi = FLOAT(Math.Pi, LONGREAL);
          PiCube = PPi * PPi * PPi;
          B = 0.5d0;
          A = (1.0d0 - B*PPi) / PiCube;        
    BEGIN
      WITH
        ThetaCube = Theta * Theta * Theta
      DO
        RETURN A * ThetaCube +  B * Theta
      END
    END ThetaFunc;

  VAR an: Arc;
  BEGIN
    WITH
      vx = OrgV(a), 
      v = vx.num,
      bar = Triang.NeighborBarycenter(a, c),
      n = Triang.VertexNormal(a, c)
    DO
      IF NOT variable[v] AND vx.exists THEN RETURN END;
      IF debug THEN Debug.LongReal("   bar =  ", bar) END;
      an := a;
      REPEAT 
        CalcTi(bar, an); 
        an := Onext(an) 
      UNTIL (an = a);
      WITH
        t = somakiti / somaki,
        ao = OrgV(a).num,
        u = LR3.Add(bar, LR3.Scale(t, n))
      DO
        IF debug THEN
          Debug.LongReal("   t =    ", LRN.T{t}); 
          Debug.LongReal("   n =    ", n);
          Debug.LongReal("   u =    ", u);
        END;
        c[ao] := u;
      END;
    END;
  END FlattenVertex;

BEGIN
END Heuristics.