MODULE GradTest;

IMPORT Math, Text, Random, R3, Fmt, Wr, Thread, 
  PSPlot, FileStream, GradMin, Praxis;
FROM Stdio IMPORT stderr;
FROM Oct IMPORT  Oprev, Onext, Sym, Rot, Tor, Enum, EnumVertices;
FROM Triang IMPORT Arc, Coords, NOrg, InitCoords,
   NormalizeCoords, PutWire;
VAR
  psf: Wr.T;

CONST 
  RGuess = 5.0;
  RPlot = 0.1;
  
PROCEDURE DebugIter(i: CARDINAL): BOOLEAN =
BEGIN
  RETURN i <= 9 OR 
    i = 10 OR i= 15 OR i=17 OR i = 19 OR i=20 OR i= 50 OR
    (i MOD 10 = 0 AND i <= 5000) OR (i MOD 200 = 0 AND i > 5000)
END DebugIter;

PROCEDURE FmtI(i: CARDINAL; nd: CARDINAL): TEXT =
BEGIN
  RETURN Fmt.Pad(Fmt.Int(i), nd, '0')
END FmtI;

PROCEDURE NumberTriang(a: Arc): CARDINAL  =
VAR NT: CARDINAL := 0;
  PROCEDURE Visit(<*UNUSED*> c: Arc) =
  BEGIN
    NT := NT + 1;
  END Visit;
BEGIN
  EnumVertices(Rot(a), Visit);
  RETURN NT;
END NumberTriang;

TYPE
  AdjMatrix =  ARRAY OF ARRAY OF BOOLEAN;

PROCEDURE CollectAdjacencies(a: Arc; NV: CARDINAL): REF AdjMatrix =
VAR adjacent := NEW(REF AdjMatrix, NV, NV);
  PROCEDURE Visit(c: Arc) =
  BEGIN
    WITH co = NOrg(c), cd = NOrg(Sym(c)) DO
      adjacent[co, cd] := TRUE;
    END;
  END Visit;
BEGIN
  FOR i := 0 TO NV-1 DO 
    FOR j := 0 TO NV-1 DO 
      adjacent[i,j] := FALSE
    END
  END;
  Enum(a, Visit, edges := TRUE);
  RETURN adjacent
END CollectAdjacencies;

PROCEDURE GradEnergy(
  a: Arc; 
  READONLY vet: ARRAY OF REAL;
  VAR dy: ARRAY OF REAL;
  <*UNUSED*> READONLY adjacent: ARRAY OF ARRAY OF BOOLEAN;
  VAR EC, EE, EL, ET: REAL;
  VAR NT: CARDINAL;
) =
VAR mi: LONGREAL := 0.0D0;
    midleL: LONGREAL := 0.0D0;
    
  PROCEDURE CurvEnergy(): REAL =
  (* Also computes mi *)
  VAR F: REAL := 0.0;
      ne: INTEGER := 0;  
  CONST zeroR3 = R3.T{0.0, 0.0, 0.0};

    PROCEDURE FuncDfDv(
      READONLY s, t, w, q: REAL; ct1, r: CARDINAL; 
      READONLY aa, bb, cc, ab, ac, bc: REAL;
      READONLY daa, dbb, dcc, dab, dac, dbc: R3.T
                     ) =
    BEGIN
      WITH
        dg = R3.add( R3.scale(dab,ac), R3.scale(dac,ab) ),
        dh = R3.add( R3.scale(daa,bc), R3.scale(dbc,aa) ),
        dm = R3.add( R3.scale(daa,bb), R3.scale(dbb,aa) ),
        dn = R3.scale(dab,2.0*ab),
        do = R3.add( R3.scale(daa,cc), R3.scale(dcc,aa) ),
        dp = R3.scale(dac,2.0*ac),
        dq = R3.sub(dg, dh),
        ds = R3.sub(dm, dn),
        dt = R3.sub(do, dp),
        du = R3.add( R3.scale(ds,t), R3.scale(dt,s) ),
        dw = R3.scale( du,(1.0/(2.0*w)) ),
        df = R3.sub( R3.scale(dw, (2.0*q)/(w*w) ), R3.scale(dq, 2.0/w) )
      DO      
        
        dy[3*ct1] := dy[3*ct1] + df[0]; 
        dy[3*ct1+1] := dy[3*ct1+1] + df[1]; 
        dy[3*ct1+2] := dy[3*ct1+2] + df[2]; 

        dy[3*r] := dy[3*r] - df[0]; 
        dy[3*r+1] := dy[3*r+1] - df[1]; 
        dy[3*r+2] := dy[3*r+2] - df[2]; 

      END;
    END FuncDfDv;
        
    PROCEDURE Visit(c: Arc) =
    BEGIN   
      WITH
        v1 = c,
        v2 = Onext(c),
        v3 = Oprev(c),
        vo = NOrg(v1),  
        v1d = NOrg(Sym(v1)),
        v2d = NOrg(Sym(v2)),
        v3d = NOrg(Sym(v3)),
        a = R3.T{vet[3*v1d]- vet[3*vo], vet[3*v1d+1]- vet[3*vo+1] ,
                     vet[3*v1d+2]- vet[3*vo+2]},
        b = R3.T{vet[3*v2d]- vet[3*vo], vet[3*v2d+1]- vet[3*vo+1] ,
                     vet[3*v2d+2]- vet[3*vo+2]},
        c = R3.T{vet[3*v3d]- vet[3*vo], vet[3*v3d+1]- vet[3*vo+1] ,
                     vet[3*v3d+2]- vet[3*vo+2]},

        aa = R3.dot(a, a), ab = R3.dot(a, b),
        ac = R3.dot(a, c), bc = R3.dot(b, c),  
        bb = R3.dot(b, b), g = ab*ac, h = aa*bc,
        cc = R3.dot(c, c), m = aa*bb, n = ab*ab,                             
        o = aa*cc, p = ac*ac, q = g - h, 
        s = m - n, t = o - p, u = s*t,                               
        w = FLOAT(Math.sqrt(FLOAT(u, LONGREAL))), 
        f = 2.0*(1.0 - q/w),

        daada = R3.scale(a, 2.0), 
        dbbdb = R3.scale(b, 2.0),  
        dccdc = R3.scale(c, 2.0),  
        dabda = b, dabdb = a,      
        dacda = c, dacdc = a,      
        dbcdb = c, dbcdc = b,     
        
        L = Math.sqrt( FLOAT(aa, LONGREAL) )     
      DO
        FuncDfDv(s, t, w, q, v1d, vo, aa, bb, cc, ab, ac, bc, daada, 
                 zeroR3, zeroR3, dabda, dacda, zeroR3);
        FuncDfDv(s, t, w, q, v2d, vo, aa, bb, cc, ab, ac, bc, zeroR3,
                 dbbdb, zeroR3, dabdb, zeroR3, dbcdb);
        FuncDfDv(s, t, w, q, v3d, vo, aa, bb, cc, ab, ac, bc, zeroR3, 
                 zeroR3, dccdc, zeroR3, dacdc, dbcdc); 
                 
        IF ABS(q/w) > 1.0 THEN 
          F := F + Math.Pi;
        ELSE
          F := F + f;
        END;
 
        mi := mi + L*L;
        midleL := midleL+L;
        INC(ne,1);                 
      END;
    END Visit; 
  BEGIN
    Enum(a, Visit, edges := TRUE); 
    mi := Math.sqrt(mi / FLOAT(ne, LONGREAL));
    midleL := midleL / FLOAT(ne, LONGREAL); 
    WITH k2 = Math.Pi * 8.0 DO
      RETURN F/k2;
    END
  END CurvEnergy;

  PROCEDURE AreaEnergy(): REAL  =
  VAR dll: ARRAY [0..2] OF R3.T;
      SareaT, alpha: REAL := 0.0;      
      
    PROCEDURE DAreaDv(
      READONLY r: CARDINAL; 
      READONLY ll: R3.T;
      READONLY areat, fAt, n: REAL;
      READONLY dll: ARRAY [0..2] OF R3.T;
                     ) =
    BEGIN
      WITH
        dm =  R3.T{R3.dot( R3.scale(ll, 2.0), dll[0]),
                   R3.dot( R3.scale(ll, 2.0), dll[1]),
                   R3.dot( R3.scale(ll, 2.0), dll[2])},
       
        dn =  R3.scale( dm,(1.0/(2.0*n)) ),                   
        darea = R3.scale(dn, 0.5),
        dfAt = R3.scale(darea, 4.0*fAt*(1.0/areat))
      DO 
        dy[3*r] := dy[3*r] + dfAt[0]; 
        dy[3*r+1] := dy[3*r+1] + dfAt[1]; 
        dy[3*r+2] := dy[3*r+2] + dfAt[2]; 
      END;
    END DAreaDv;
  
    PROCEDURE Visit(c: Arc) =
    BEGIN
      WITH
        k = NOrg(Tor(c)),
        d = Onext(c), l = NOrg(Tor(d)),
        e = Onext(d), m = NOrg(Tor(e)),
        a = R3.T{vet[3*k], vet[3*k+1], vet[3*k+2]},
        b = R3.T{vet[3*l], vet[3*l+1], vet[3*l+2]},
        c = R3.T{vet[3*m], vet[3*m+1], vet[3*m+2]},        
        v1 = R3.sub(b, a), v2 = R3.sub(c, a),     
        ll = R3.cross(v1, v2), 
        dx = R3.T{1.0, 0.0, 0.0}, dy = R3.T{0.0, 1.0, 0.0},
        dz = R3.T{0.0, 0.0, 1.0},
        n = FLOAT(Math.sqrt(FLOAT(R3.dot(ll, ll),LONGREAL))),
        areat = 0.5*n,
        fAt = FLOAT(Math.log(FLOAT( (areat*areat)/(alpha*alpha), LONGREAL )))
      DO 
         

        (* Compute: dlldax, dllday, dlldaz *)
        
        dll[0] := R3.add( R3.cross(c, dx), R3.cross(dx, b) );
        dll[1] := R3.add( R3.cross(c, dy), R3.cross(dy, b) );
        dll[2] := R3.add( R3.cross(c, dz), R3.cross(dz, b) );
        
        DAreaDv(k, ll, areat, fAt, n, dll);
        
        dll[0] := R3.add( R3.cross(dx, c), R3.cross(a, dx) );
        dll[1] := R3.add( R3.cross(dy, c), R3.cross(a, dy) );
        dll[2] := R3.add( R3.cross(dz, c), R3.cross(a, dz) );
        
        DAreaDv(l, ll, areat, fAt, n, dll);
        
        dll[0] := R3.add( R3.cross(b, dx), R3.cross(dx, a) );
        dll[1] := R3.add( R3.cross(b, dy), R3.cross(dy, a) );
        dll[2] := R3.add( R3.cross(b, dz), R3.cross(dz, a) );
        
        DAreaDv(m, ll, areat, fAt, n, dll);
      
        SareaT := SareaT + fAt*fAt;
      END; 
    END Visit;
  BEGIN
    alpha := 4.0*Math.Pi/FLOAT(NT); 
    EnumVertices(Rot(a), Visit);
    RETURN SareaT;
  END AreaEnergy;
 
BEGIN 
  EC := CurvEnergy();
  EE := 0.0; 
  EL := AreaEnergy()/(FLOAT(NT)*FLOAT(Math.log(2.0D0)*Math.log(2.0D0)));  
  ET := EC + EE + EL;
END GradEnergy;

PROCEDURE GradTOpt(
 a: Arc; 
  VAR coords: Coords; 
  sname: TEXT; 
  NGradIter: CARDINAL
) =

VAR NV: CARDINAL := NUMBER(coords);
    I, NT: CARDINAL := 0;
    NCols: CARDINAL := 3*NV;
    vet := NEW(REF ARRAY OF REAL, NCols);
    dy := NEW(REF ARRAY OF REAL, NCols);
    coordst := NEW(REF ARRAY OF ARRAY OF REAL, NV, 3); 
    adjacent := CollectAdjacencies(a, NV);    
    Y: REAL := 0.0;

  PROCEDURE PutWireTrianglestoGrad(wr: Wr.T; a: Arc; READONLY coords: Coords;
                             READONLY coordst: Coords) =
  TYPE Color = ARRAY [0..2] OF INTEGER;
  VAR s := R3.T{Random.Real(), Random.Real(), Random.Real()};

   <* FATAL Wr.Failure, Thread.Alerted *>
    PROCEDURE Visit(c: Arc) =
    BEGIN
      WITH
        k = NOrg(Tor(c)),
        d = Onext(c), l = NOrg(Tor(d)),
        e = Onext(d), m = NOrg(Tor(e)),
        color = Color{255, 255, 0}
      DO
        Wr.PutText(wr, "#   square \n\n");
        Wr.PutText( wr, Fmt.Int(color[0]) &" "& Fmt.Int(color[1])
                    &" "& Fmt.Int(color[2]) );
        Wr.PutText(wr, "  # color \n\n");
        Wr.PutText( wr, Fmt.Real(coords[k][0]*1000.0 +s[0]) &" "&
               Fmt.Real(coords[k][1]*1000.0+s[1]) &" "& Fmt.Real(coords[k][2]*1000.0+s[2]) &"\n");
         Wr.PutText( wr,  Fmt.Real(coordst[k][0]*1000.0+s[0]) &" "&
                Fmt.Real(coordst[k][1]*1000.0+s[1]) &" "& Fmt.Real(coordst[k][2]*1000.0+s[2])&"\n" );       
        Wr.PutText( wr,  Fmt.Real(coords[k][0]*1000.0-s[0]) &" "&
               Fmt.Real(coords[k][1]*1000.0-s[1]) &" "& Fmt.Real(coords[k][2]*1000.0-s[2])&"\n" );
         Wr.PutText( wr,  Fmt.Real(coordst[k][0]*1000.0-s[0]) &" "&
               Fmt.Real(coordst[k][1]*1000.0-s[1]) &" "& Fmt.Real(coordst[k][2]*1000.0-s[2]) );  
        Wr.PutText(wr, "\n\n");

        Wr.PutText(wr, "#   square \n\n");
        Wr.PutText( wr, Fmt.Int(color[0]) &" "& Fmt.Int(color[1])
                    &" "& Fmt.Int(color[2]) );
        Wr.PutText(wr, "  # color \n\n");
        Wr.PutText( wr, Fmt.Real(coords[l][0]*1000.0+s[0]) &" "&
               Fmt.Real(coords[l][1]*1000.0+s[1]) &" "& Fmt.Real(coords[l][2]*1000.0+s[2]) &"\n ");
         Wr.PutText( wr,  Fmt.Real(coordst[l][0]*1000.0+s[0]) &" "&
                Fmt.Real(coordst[l][1]*1000.0+s[1]) &" "& Fmt.Real(coordst[l][2]*1000.0+s[2])&"\n" );       
        Wr.PutText( wr,  Fmt.Real(coords[l][0]*1000.0-s[0]) &" "&
               Fmt.Real(coords[l][1]*1000.0-s[1]) &" "& Fmt.Real(coords[l][2]*1000.0-s[2])&"\n" );
        Wr.PutText( wr,  Fmt.Real(coordst[l][0]*1000.0-s[0]) &" "&
               Fmt.Real(coordst[l][1]*1000.0-s[1]) &" "& Fmt.Real(coordst[l][2]*1000.0-s[2]) );
        Wr.PutText(wr, "\n\n");

        Wr.PutText(wr, "#   square \n\n");
        Wr.PutText( wr, Fmt.Int(color[0]) &" "& Fmt.Int(color[1])
                    &" "& Fmt.Int(color[2]) );
        Wr.PutText(wr, "  # color \n\n");
        Wr.PutText( wr, Fmt.Real(coords[m][0]*1000.0+s[0]) &" "&
               Fmt.Real(coords[m][1]*1000.0+s[1]) &" "& Fmt.Real(coords[m][2]*1000.0+s[2]) &"\n");
        Wr.PutText( wr,   Fmt.Real(coordst[m][0]*1000.0+s[0]) &" "&
                Fmt.Real(coordst[m][1]*1000.0+s[1]) &" "& Fmt.Real(coordst[m][2]*1000.0+s[2])&"\n" );       
        Wr.PutText( wr,   Fmt.Real(coords[m][0]*1000.0-s[0]) &" "&
               Fmt.Real(coords[m][1]*1000.0-s[1]) &" "& Fmt.Real(coords[m][2]*1000.0-s[2])&"\n" );
        Wr.PutText( wr,  Fmt.Real(coordst[m][0]*1000.0-s[0]) &" "&
               Fmt.Real(coordst[m][1]*1000.0-s[1]) &" "& Fmt.Real(coordst[m][2]*1000.0-s[2]) ); 
        Wr.PutText(wr, "\n\n");

      END;
    END Visit;
  BEGIN
    s := R3.scale(s, 15.0);    
    EnumVertices(a, Visit);
  END PutWireTrianglestoGrad;

  PROCEDURE WriteWireFrametoGrad(a: Arc; vet: ARRAY OF REAL; dy: ARRAY OF REAL;
                                 filename: TEXT) =
  VAR SGrad: REAL := 0.0;                                 
  <* FATAL Wr.Failure, Thread.Alerted *>
  BEGIN

    WITH 
      name = filename & ".poly",
      wr = FileStream.OpenWrite(name)
    DO
      FOR i := 0 TO NV-1 DO 
        coords[i][0] := vet[3*i]; 
        coords[i][1] := vet[3*i+1];
        coords[i][2] := vet[3*i+2];
      END;
        
      FOR i := 0 TO NV-1 DO 
        SGrad := SGrad + dy[3*i]*dy[3*i] + dy[3*i+1]*dy[3*i+1] + 
                 dy[3*i+2]*dy[3*i+2];
      END;
      SGrad := FLOAT(Math.sqrt( FLOAT( (2.0*SGrad)/FLOAT(3*NV-1), LONGREAL) ));
      
      NormalizeCoords(coords);
      FOR i := 0 TO NV-1 DO 
        coordst[i][0] := dy[3*i]/SGrad;
        coordst[i][1] := dy[3*i+1]/SGrad;
        coordst[i][2] := dy[3*i+2]/SGrad;
      END;
      
      PutWire(wr, Rot(a), coords);

      FOR i := 0 TO NV-1 DO 
        coordst[i] := R3.sub( coords[i], R3.scale(coordst[i], 4.0) ); 
      END;
      PutWireTrianglestoGrad(wr, Rot(a), coords, coordst^);  
      Wr.Close(wr)
    END
  END WriteWireFrametoGrad;


  <*UNUSED*>
  PROCEDURE Fonc(READONLY vet: ARRAY OF REAL; VAR y: REAL; VAR dy: ARRAY OF REAL; ITer: CARDINAL) = 

  VAR EC, EE, EL, ET: REAL := 0.0;
      filename: TEXT;
  
  BEGIN 
    WITH 
      writeit = DebugIter(ITer)
    DO        
      FOR i := 0 TO 3*NV-1 DO 
        dy[i] := 0.0;
      END;

      IF writeit THEN 
        filename := sname & "-" & FmtI(ITer, 5)  ;
        WriteWireFrametoGrad(a, vet, dy, filename);
      END;
      GradEnergy(a, vet, dy, adjacent^, EC, EE, EL, ET, NT); 
      y := ET;
      
      FOR i := 0 TO NV-1 DO
        IF I # i THEN 
          dy[3*i] := 0.0;
          dy[3*i+1] := 0.0;
          dy[3*i+2] := 0.0;
        END;
      END;
      
      IF writeit THEN 
        filename := filename & "SM";
        WriteWireFrametoGrad(a, vet, dy, filename);
      END;
      IF writeit THEN
        Wr.PutText(stderr, filename & "\n");
        Wr.PutText(stderr, "  EE = " & Fmt.Real(EE));
        Wr.PutText(stderr, "  EC = " & Fmt.Real(EC)& "  ");
        Wr.PutText(stderr, "  EL = " & Fmt.Real(EL)& "  ");        
        Wr.PutText(stderr, "  ET = " & Fmt.Real(ET)& "  ");
        Wr.PutText(stderr, "\n");   
      END;
    END
  END Fonc;
  
  PROCEDURE Func(READONLY vet: ARRAY OF REAL; VAR y: REAL; VAR dy: ARRAY OF REAL; ITer: CARDINAL) = 

  VAR EC, EE, EL, ET: REAL := 0.0;
      filename: TEXT;
  
  BEGIN 
    WITH 
      writeit = DebugIter(ITer) 
    DO        
      FOR i := 0 TO 3*NV-1 DO 
        dy[i] := 0.0;
      END;

      IF writeit THEN 
        filename := sname & "-" & FmtI(ITer, 5)  ;
        WriteWireFrametoGrad(a, vet, dy, filename);
      END;
      PlotPoint(vet[0], vet[1], FALSE);            
      GradEnergy(a, vet, dy, adjacent^, EC, EE, EL, ET, NT);
      y := ET;
      IF writeit THEN 
        filename := filename & "AE";
        WriteWireFrametoGrad(a, vet, dy, filename);
      END;
      IF writeit THEN
        Wr.PutText(stderr, filename & "\n");
        Wr.PutText(stderr, "  EE = " & Fmt.Real(EE));
        Wr.PutText(stderr, "  EC = " & Fmt.Real(EC)& "  ");
        Wr.PutText(stderr, "  EL = " & Fmt.Real(EL)& "  ");        
        Wr.PutText(stderr, "  ET = " & Fmt.Real(ET)& "  ");
        Wr.PutText(stderr, "\n");   
      END;
    END
  END Func;

  PROCEDURE InitGuess (VAR vet: ARRAY OF REAL) =
  BEGIN
    FOR i := 0 TO NV-1 DO
      FOR j := 0 TO 2 DO 
        vet[3*i+j] := coords[i][j];
      END;        
    END;
  END InitGuess;

CONST NIts = 100;
<* FATAL Wr.Failure, Thread.Alerted *>
BEGIN
  InitPlot(0.0, 0.1, 0.1);
  NT := NumberTriang(a);
  InitCoords(coords);
  
(*  FOR i := 0 TO NIts DO 
    SmoothVertices(a, coords);
    NormalizeCoords(a, coords);
  END;
  WITH b = Baricentro(coords) DO 
    coords[0][0] := coords[0][0] - 0.9*b[0];
    coords[0][1] := coords[0][1] - 0.9*b[1];
    coords[0][2] := coords[0][2] - 0.9*b[2]; 
  END; *)  
  InitGuess(vet^); 
(*
  FOR i := 0 TO NV-1 DO 
    I := i;
    GradMin.Minimize(
      Fonc, 
      vet^,
      dy^,
      y := Y,
      maxCalls := NIts, (*NGradIter*)
      minStep := 1.0e-8,
      maxStep := 1.0e+30,
      report := Report,
      verbose := TRUE,
      normalize := Normalize      
    );   
  END; *)  
  GradMin.Minimize(
    Func, 
    vet^,
    dy^,
    y := Y,
    maxCalls := NGradIter,
    minStep := 1.0e-8,
    maxStep := 1.0e+30,
    report := Report,
    verbose := TRUE,
    normalize := Normalize      
  ); 
  EndPlot();
  FOR i := 0 TO NV-1 DO 
    coords[i][0] := vet[3*i]; 
    coords[i][1] := vet[3*i+1];
    coords[i][2] := vet[3*i+2];
  END;

END GradTOpt;

PROCEDURE PraxEnergy(
  a: Arc; 
  READONLY vet: ARRAY OF REAL;
  READONLY adjacent: ARRAY OF ARRAY OF BOOLEAN;
  VAR EC, EE, EL, ET: REAL;
  NT: CARDINAL;
) =
VAR mi: LONGREAL := 0.0D0;
    midleL: LONGREAL := 0.0D0;
    SareaT: REAL := 0.0;
    
  PROCEDURE CurvEnergy(): REAL =
  (* Also computes mi *)
  VAR F, alpha: REAL := 0.0;
      ne: INTEGER := 0;  
      
    PROCEDURE Visit(c: Arc) =
    BEGIN   
      WITH
        v1 = c,
        v2 = Onext(c),
        v3 = Oprev(c),
        vo = NOrg(v1),  
        v1d = NOrg(Sym(v1)),
        v2d = NOrg(Sym(v2)),
        v3d = NOrg(Sym(v3)),
        a = R3.T{vet[3*v1d]- vet[3*vo], vet[3*v1d+1]- vet[3*vo+1] ,
                     vet[3*v1d+2]- vet[3*vo+2]},
        b = R3.T{vet[3*v2d]- vet[3*vo], vet[3*v2d+1]- vet[3*vo+1] ,
                     vet[3*v2d+2]- vet[3*vo+2]},
        c = R3.T{vet[3*v3d]- vet[3*vo], vet[3*v3d+1]- vet[3*vo+1] ,
                     vet[3*v3d+2]- vet[3*vo+2]},
                     
        aa = R3.dot(a, a), bb = R3.dot(b, b), cc = R3.dot(c, c), 
        ab = R3.dot(a, b), ac = R3.dot(a, c), bc = R3.dot(b, c), 
        L = Math.sqrt( FLOAT(aa, LONGREAL) ),
        g = ab*ac,  h = aa*bc, m = aa*bb, 
        n = ab*ab, o = aa*cc, p = ac*ac, 
        q = g - h, s = m - n, t = o - p, 
        u = s*t, w = FLOAT(Math.sqrt(FLOAT(u, LONGREAL))), 
        f = 2.0*(1.0 - q/w)      
      DO
        IF ABS(q/w) > 1.0 THEN 
          F := F + Math.Pi;
        ELSE
          F := F + f;
        END;
        mi := mi + L*L;
        midleL := midleL+L;
        INC(ne,1);
      END;
    END Visit; 
  BEGIN
    Enum(a, Visit, edges := TRUE); 
    mi := Math.sqrt(mi / FLOAT(ne, LONGREAL));
    midleL := midleL / FLOAT(ne, LONGREAL); 
    WITH k2 = Math.Pi * 8.0 DO
      RETURN F/k2;
    END
  END CurvEnergy;
  
  PROCEDURE AreaEnergy(): REAL  =
  VAR SareaT, alpha: REAL := 0.0;      
  
    PROCEDURE Visit(c: Arc) =
    BEGIN
      WITH
        k = NOrg(Tor(c)),
        d = Onext(c), l = NOrg(Tor(d)),
        e = Onext(d), m = NOrg(Tor(e)),
        a = R3.T{vet[3*k], vet[3*k+1], vet[3*k+2]},
        b = R3.T{vet[3*l], vet[3*l+1], vet[3*l+2]},
        c = R3.T{vet[3*m], vet[3*m+1], vet[3*m+2]},        
        v1 = R3.sub(b, a), v2 = R3.sub(c, a),     
        ll = R3.cross(v1, v2), 
        n = FLOAT(Math.sqrt(FLOAT(R3.dot(ll, ll),LONGREAL))),
        areat = 0.5*n,
        fAt = FLOAT(Math.log(FLOAT( (areat*areat)/(alpha*alpha), LONGREAL )))
      DO 
        SareaT := SareaT + fAt*fAt;
      END; 
    END Visit;
  BEGIN
    alpha := 4.0*Math.Pi/FLOAT(NT); 
    EnumVertices(Rot(a), Visit);
    RETURN SareaT;
  END AreaEnergy;
  
BEGIN 
  EC := CurvEnergy();
  EE := 0.0; 
  EL := 0.0;(*AreaEnergy()/(FLOAT(NT)*FLOAT(Math.log(2.0D0)*Math.log(2.0D0)));*)
  ET := EC + EE + EL;
END PraxEnergy;

PROCEDURE PraxTOpt(
 a: Arc; 
  VAR coords: Coords; 
  sname: TEXT; 
  NPraxIter: CARDINAL
) =

VAR NV: CARDINAL := NUMBER(coords);
    NT: CARDINAL := 0;
    NCols: CARDINAL := 3*NV;
    vet := NEW(REF ARRAY OF REAL, NCols);
    adjacent := CollectAdjacencies(a, NV);    
    Y: REAL := 0.0;

    PROCEDURE WriteWireFrametoPrax(a: Arc; vet: ARRAY OF REAL; filename: TEXT)=
    <* FATAL Wr.Failure, Thread.Alerted *>
    BEGIN
      
      WITH 
        name = filename & ".poly",
        wr = FileStream.OpenWrite(name)
      DO
        FOR i := 0 TO NV-1 DO 
(*          Wr.PutText(stderr, "  i= " & Fmt.Int(i)& "  ");
          Wr.PutText(stderr, "  x = " & Fmt.Real(vet[3*i])& "  ");
          Wr.PutText(stderr, "  x = " & Fmt.Real(vet[3*i+1])& "  "); 
          Wr.PutText(stderr, "  x = " & Fmt.Real(vet[3*i+2])& "\n"); 
  *)      
          coords[i][0] := vet[3*i]; 
          coords[i][1] := vet[3*i+1];
          coords[i][2] := vet[3*i+2];
        END;
        NormalizeCoords(coords);
        PutWireTriangles(wr, Rot(a), coords);
        Wr.Close(wr)
      END
    END WriteWireFrametoPrax;

  PROCEDURE Func(VAR vet: ARRAY OF REAL; VAR y: REAL; ITer: CARDINAL) = 

  VAR EC, EE, EL, ET: REAL;
      filename: TEXT;
  
  BEGIN 
    WITH 
      writeit = DebugIter(ITer) 
    DO        
      IF writeit THEN 
        filename := sname & "-" & FmtI(ITer, 5)  ;
        WriteWireFrametoPrax(a, vet, filename);
      END;
      PlotPoint(vet[0], vet[1], FALSE);  
      PraxEnergy(a, vet, adjacent^, EC, EE, EL, ET, NT);
      y := ET;
      IF writeit THEN 
        filename := filename & "AE";
        WriteWireFrametoPrax(a, vet, filename);
      END;
      IF writeit THEN
        Wr.PutText(stderr, filename & "\n");
        Wr.PutText(stderr, "  EE = " & Fmt.Real(EE));
        Wr.PutText(stderr, "  EC = " & Fmt.Real(EC)& "  ");
        Wr.PutText(stderr, "  EL = " & Fmt.Real(EL)& "  ");        
        Wr.PutText(stderr, "  ET = " & Fmt.Real(ET)& "  ");
        Wr.PutText(stderr, "\n");   
      END;
    END
  END Func;

  PROCEDURE InitGuess (VAR vet: ARRAY OF REAL) =
  BEGIN
    FOR i := 0 TO NV-1 DO
      FOR j := 0 TO 2 DO 
        vet[3*i+j] := coords[i][j];
      END;        
    END;
  END InitGuess;

CONST NIts = 15;
<* FATAL Wr.Failure, Thread.Alerted *>
BEGIN
  NT := NumberTriang(a);
  InitCoords(coords);
(*  FOR i := 0 TO NIts DO 
    SmoothVertices(a, coords);
    NormalizeCoords(a, coords);
  END;
  coords[0][0] := coords[0][0] + 0.9;
  coords[0][1] := coords[0][1] + 0.9;
  coords[0][2] := coords[0][2] + 0.9;  
  coords[10][0] := coords[10][0] - 0.9;
  coords[10][1] := coords[10][1] - 0.9;
  coords[10][2] := coords[10][2] - 0.9;
  coords[6][0] := coords[NV-1][0] + 0.5;
  coords[6][1] := coords[NV-1][1] + 0.5;
  coords[6][2] := coords[NV-1][2] + 0.5;  
 *) 
  InitGuess(vet^); 

  Y := Praxis.Minimize(
    Func,
    vet^,
    fx := Y,
    t0 := 1.0e-5,
    scbd := 1.0,
    illc := FALSE,
    ktm := 1,
    prin := 0,
    maxCalls := NPraxIter,
    machep := 1.0e-15,
    maxStep := 1.0e+2,
    report := Report,
    normalize := Normalize
  );  
 
  FOR i := 0 TO NV-1 DO 
    coords[i][0] := vet[3*i]; 
    coords[i][1] := vet[3*i+1];
    coords[i][2] := vet[3*i+2];
  END;

END PraxTOpt;

VAR (* Used by PlotPoint: *)
  prevxx, prevyy: LONGREAL;
  firstPoint: BOOLEAN;

PROCEDURE PlotPoint(x, y: REAL; black: BOOLEAN) =
  BEGIN
    WITH 
      xx = FLOAT(x, LONGREAL),
      yy = FLOAT(y, LONGREAL),
      mm = FLOAT((2.0 * RPlot)/(6.0 * 24.5), LONGREAL)
    DO
      IF firstPoint THEN
        firstPoint := FALSE
      ELSE
        PSPlot.DrawSegment(psf, prevxx, prevyy, xx, yy);
      END;
      IF black THEN
        PSPlot.FillCircle(psf, xx, yy, 0.4D0 * mm, 0.0)
      ELSE
        PSPlot.DrawCircle(psf, xx, yy, 0.6D0 * mm)
      END;
      prevxx := xx;
      prevyy := yy
    END
  END PlotPoint;

PROCEDURE Normalize(<*UNUSED*> VAR x: ARRAY OF REAL) =
  BEGIN
  END Normalize;


PROCEDURE Report (READONLY  y: REAL; READONLY ni: CARDINAL): BOOLEAN =
  <* FATAL Wr.Failure, Thread.Alerted *>
  BEGIN 
    PrintPoint("down ", y, ni);
    RETURN FALSE
  END Report;
PROCEDURE PrintPoint (
  msg: TEXT; 
  READONLY u: REAL;
  READONLY ni: CARDINAL;
) =
<* FATAL Wr.Failure, Thread.Alerted *>
VAR M := Text.Length(msg);
BEGIN  
  FOR i := 0 TO M DO Wr.PutChar(stderr, ' ') END;
  Wr.PutText(stderr, "Int:    " & Fmt.Int(ni));
  Wr.PutText(stderr, "\n");
  
  FOR i := 0 TO M DO Wr.PutChar(stderr, ' ') END;
  Wr.PutText(stderr, "f =    " & Fmt.Pad(Fmt.Real(u), 12));
  Wr.PutText(stderr, "\n");    
END PrintPoint;

PROCEDURE InitPlot(cx, cy: REAL; r: REAL) =
  <* FATAL Wr.Failure, Thread.Alerted *>
  BEGIN
    psf := FileStream.OpenWrite("gmt.ps");
    PSPlot.BeginFile(psf);
    WITH 
      ccx = FLOAT(cx, LONGREAL),
      ccy = FLOAT(cy, LONGREAL),
      rr = FLOAT(r, LONGREAL)
    DO
      PSPlot.BeginPage(psf, 1, ccx-rr, ccx+rr, ccy-rr, ccy+rr, 1, 1)
    END;
    PSPlot.SetPen(psf, 0.0, 0.1, 0.0, 0.0);
    firstPoint := TRUE;
  END InitPlot;
  
PROCEDURE EndPlot () =
  BEGIN
    PSPlot.DrawFrame(psf);
    PSPlot.EndPage(psf);
    PSPlot.EndFile(psf);
  END EndPlot;
  
BEGIN
  
END GradTest.