-- December 10, 1982 2:25 pm
-- program junoSolverImplB.mesa, coded July 1981 by Greg Nelson

-- This is the solver for an interactive graphics program that allows a user to 
-- specify the positions of points by declaring geometrical contraints that
-- the positions are to satisfy. The program solves constraints (which can be
-- viewed as simultaneous polynomial equations) by an n-dimensional version of
-- Newton's method for finding the root of a differentiable function.
-- Last Edited by: Gnelson, October 11, 1983 7:00 pm

DIRECTORY  RealFns, JunoStorage, JunoSolver, JunoGraphics, Real, IO, JunoButtons;
           

JunoSolverImplB: PROGRAM
  IMPORTS  RealFns, JunoStorage, JunoGraphics, Real
  EXPORTS JunoSolver  = 

BEGIN OPEN JunoStorage, JunoSolver, JG: JunoGraphics;

displayingMotions: PUBLIC BOOL _ FALSE;
loggingPivots: BOOL _ FALSE;

rowmax: INTEGER = 100;

colmax: INTEGER = 100;

Tableau: TYPE = ARRAY[1..rowmax] OF ARRAY[1..colmax] OF REAL;

n,m: PUBLIC INT; -- the tableau a has n relevant rows and m relevant columns.

a: REF Tableau _ NEW[Tableau];

Tag: TYPE = {x, y, eq, geq};

TableauLabel: TYPE = RECORD [point: PointPtr, tag: Tag];

  -- each rows and column of the matrix represents a single scalar variable.
  -- If the row or column is labelled with the TableauLabel (p, x),
  -- then the scalar variable that the row or column represents is
  -- the x coordinate of the point p; similarly (p, y) means the y coordinate.
  -- The label (*, eq) means the scalar variable is a "slack variable" associated
  -- with some constraint; the variable should have the value zero in any
  -- solution.  The label (*, geq) menas the scalar variable should have a
  -- value greater than or equal to zero in any solution.   

rowhead: ARRAY[1..rowmax] OF TableauLabel;

colhead: ARRAY[1..colmax] OF TableauLabel;

TooManyConstraints: SIGNAL = CODE;

TooManyConstrainedPoints: SIGNAL = CODE;

Square: PROCEDURE[x:REAL] RETURNS [REAL] =  BEGIN RETURN [x*x] END;

Sqrt: PROCEDURE[x:REAL] RETURNS [REAL] =  BEGIN RETURN [RealFns.SqRt[x]] END;

Solve: PUBLIC PROC[epsilon: REAL] RETURNS [success: BOOLEAN] =
  {n: INT _ 0;
   SavePositions[];
   [] _ NewtonRaphson[];
   WHILE n < 20 AND SomePointMoved[] DO
     IF displayingMotions THEN DisplayMotions[]; 
     n _ n + 1;
     SavePositions[];
     [] _ NewtonRaphson[]
   ENDLOOP;
   RETURN [n < 20]}; 

SavePositions: PROC =
 {p: PointPtr _ pointLpad.link; 
  UNTIL p = pointRpad DO p.oldx _ Real.Fix[p.x]; p.oldy _ Real.Fix[p.y]; p _ p.link ENDLOOP};

SomePointMoved: PROC RETURNS [BOOL] =
  {p: PointPtr _ pointLpad.link;
   UNTIL p = pointRpad DO 
     IF p.oldx # Real.Fix[p.x] OR p.oldy # Real.Fix[p.y] THEN RETURN [TRUE];
     p _ p.link
   ENDLOOP;
   RETURN [FALSE]}; 
 
 DisplayMotions: PROC =
  {p: PointPtr _ pointLpad.link;
   UNTIL p = pointRpad DO 
     IF p.oldx # Real.Fix[p.x] OR p.oldy # Real.Fix[p.y]
        THEN JG.DrawEdge[p.oldx, p.oldy, p.x, p.y];
     p _ p.link
   ENDLOOP};
   
   
NewtonRaphson: PROC RETURNS [success: BOOLEAN] =
BEGIN
  
  NewtonStep: PROC[error: REAL,  gradient: TangentVector, tag: Tag] =
    -- processes one constraint whose current value is ERROR and whose current
    -- gradient is GRADIENT. Processes means adds row to matrix.
    BEGIN
      IF gradient = NIL THEN RETURN;
      n _ n + 1; -- add new row
      IF n > rowmax THEN SIGNAL TooManyConstraints[];
      rowhead[n] _ [point: NIL, tag: tag];
      FOR j: INTEGER IN [2..m] DO a[n][j] _ 0.0 ENDLOOP;
      a[n][1] _ - error;
      UNTIL gradient = NIL DO
        IF ABS[gradient.x] > .000001 AND (NOT gradient.head.fixed) 
                                          AND (NOT gradient.head.tempfixed) THEN
	  {IF gradient.head.xheader = 0 
	   THEN -- add new column: 
	     {m _ m + 1;
	      IF m > colmax THEN SIGNAL TooManyConstrainedPoints[];
	      FOR i: INTEGER IN [1..n] DO a[i][m] _ 0.0 ENDLOOP;
	      gradient.head.xheader _ m;
	      colhead[m] _ [point:gradient.head, tag: x]};
	   a[n][gradient.head.xheader] _ a[n][gradient.head.xheader] + gradient.x};
	   
	 IF ABS[gradient.y] > .000001 AND (NOT gradient.head.fixed) 
	                                   AND (NOT gradient.head.tempfixed) THEN
	  {IF gradient.head.yheader = 0 
	   THEN -- add new column: 
	     {m _ m + 1;
	      IF m > colmax THEN SIGNAL TooManyConstrainedPoints[];
	      FOR i: INTEGER IN [1..n] DO a[i][m] _ 0.0 ENDLOOP;
	      gradient.head.yheader _ m;
	      colhead[m] _ [point:gradient.head, tag: y]};
	   a[n][gradient.head.yheader] _ a[n][gradient.head.yheader] + gradient.y};
	 
	 gradient _ gradient.tail;
       ENDLOOP;
     END;
     
 ConsTv: PROC[head:PointPtr, x:REAL, y:REAL, tail:TangentVector] RETURNS [TangentVector] =
  {tv:TangentVector _ NewTv[];
   tv.head _ head;
   tv.x _ x;
   tv.y _ y;
   tv.tail _ tail;
   RETURN [tv]};
     
 GradHor: PROC[p1, p2: PointPtr] RETURNS [tv:TangentVector] =
  -- return gradient of function p1.y - p2.y:
  {tv _ ConsTv[head:p1, x:0, y:1, tail:ConsTv[head:p2, x:0, y:-1, tail:NIL]]};

GradVer: PROC[p1, p2: PointPtr] RETURNS [tv:TangentVector] =
  -- return gradient of function p1.x - p2.x:
  {tv _ ConsTv[p1, 1, 0, ConsTv[p2, -1, 0, NIL]]};

GradCong: PROC[p1, p2, p3, p4: PointPtr] RETURNS [tv:TangentVector] =
  {tv _ ConsTv[p1, p1.x - p2.x, p1.y - p2.y,
          ConsTv[p2, p2.x - p1.x, p2.y - p1.y,
	    ConsTv[p3, p4.x - p3.x, p4.y - p3.y,
	      ConsTv[p4, p3.x - p4.x, p3.y - p4.y, NIL]]]]};

GradLin: PROC[pi, pj, pk, pl: PointPtr] RETURNS [tv: TangentVector] =
  {tv _ ConsTv[pi, pl.y - pk.y, pk.x - pl.x,
          ConsTv[pj, pk.y - pl.y, pl.x - pk.x,
	      ConsTv[pk, pi.y - pj.y, pj.x - pi.x, 
	      ConsTv[pl,  pj.y - pi.y, pi.x - pj.x, NIL]]]]};

GradCC: PROC[p1, p2, p3: PointPtr] RETURNS [tv: TangentVector] =
  {tv _ ConsTv[p1, p3.y - p2.y, p2.x-p3.x,
          ConsTv[p2, p1.y-p3.y, p3.x-p1.x,
	    ConsTv[p3, p2.y-p1.y,p1.x-p2.x, NIL]]]};
	
  p: PointPtr _ pointLpad.link;
  UNTIL p = pointRpad DO p.xheader _ 0; p.yheader _ 0; p _ p.link ENDLOOP;
   
  success _ TRUE;
  n _ 0;
  m _ 1; -- matrix has n rows and m columns.
  -- column 1 is the constant column.
 
  BEGIN -- handle counter-clockwise constraints:
    p: CCPtr _ ccLpad.link;
    UNTIL p = ccRpad DO
      NewtonStep[(p.j.y - p.i.y) * (p.k.x - p.i.x) - (p.j.x - p.i.x) * (p.k.y - p.i.y) + 500.0,
                 GradCC[p.i, p.j, p.k], geq];
      p _ p.link;
    ENDLOOP;
  END;

    BEGIN  -- handle string constraints:
    p: StringPtr _ stringLpad.link;
    UNTIL p = stringRpad DO
      NewtonStep [p.b4.x - p.b3.x - p.width, GradVer[p.b4, p.b3], eq];
      NewtonStep [p.b4.y - p.b3.y, GradHor[p.b4, p.b3], eq];
      p _ p.link;
    ENDLOOP;
  END;
 
  BEGIN  -- handle horizontal constraints:
    p: HorPtr _ horLpad.link;
    UNTIL  p = horRpad DO
      NewtonStep[p.i.y - p.j.y, GradHor[p.i, p.j], eq];
      p _ p.link
    ENDLOOP;
  END;

  BEGIN  -- handle vertical constraints:
    p: VerPtr _ verLpad.link;
    UNTIL  p = verRpad DO
      NewtonStep[p.i.x - p.j.x, GradVer[p.i, p.j], eq];
      p _ p.link;
    ENDLOOP;
  END;

  BEGIN -- handle parallel constraints:
    p: LinPtr _ lineLpad.link;
    UNTIL p = lineRpad DO
      NewtonStep[(p.j.x - p.i.x)*(p.k.y - p.l.y) + (p.l.x - p.k.x)*(p.j.y - p.i.y),
                 GradLin[p.i, p.j, p.k, p.l], eq];
      p _ p.link;
    ENDLOOP;
  END;

  BEGIN -- handle congruence constraints:
    p: CongPtr _ congLpad.link;
    UNTIL p = congRpad DO
      NewtonStep[0.5 * (Square[p.j.x - p.i.x] + Square[p.j.y - p.i.y]
                          - Square[p.l.x - p.k.x] - Square[p.l.y - p.k.y]),
	         GradCong[p.i, p.j, p.k, p.l], eq];
      p _ p.link;
    ENDLOOP;
  END;
   
  -- Now invert matrix, and move points:
  
   BEGIN
    i, j, k, l: INTEGER;
    pp: REAL;
    -- k is the row in which we are pivoting.
    -- l is the column in which we are pivoting.
    -- i and j are miscellaneous row and column indices respectively
    -- p is the reciprocal of the pivot element; also used as temp for swapping.
    
    FOR k IN [1..n] DO
      IF rowhead[k].tag = eq OR rowhead[k].tag = geq AND a[k][1] < -.01
      THEN {-- set l so a[k,l] is largest of a[k,1], a[k, 2], a[k, 3], excluding
               -- columns in which we have already pivoted.
               pp _ 0;
               l _ 0;
               FOR j IN [2 .. m] 
                 DO IF ABS[a[k][j]] >= pp AND colhead[j].tag IN [x .. y] 
	              THEN {l _ j; pp _ ABS[a[k][l]]}
                 ENDLOOP;
               -- We will pivot at a[k,l], if it exists and is not too small, and is legal.
               -- First check that it is not too small:
               IF l = 0 OR ABS[a[k][l]] < .001 THEN
                 IF ABS[a[k][1]] > .01
	           -- then this row is an unsatisfiable constraint so: 
	           THEN {LOOP}
	           ELSE LOOP; -- row is identically zero; constraint redundant.
	           -- Next we must check that pivoting at a[k, l] will not ruin any
	           -- inequalities that are currently satisfied.  If there are any such inequalities,
	           -- we change k to the index of one of them (namely, the first that would become
	           -- unsatisfied), and pivot there instead.  That is the function of the following block:
	  
	           { -- adjust k to account for inequalities:
	             kk: INTEGER _ k; -- save k 
	             FOR i: INT IN [1..n] DO 
	                IF i # kk AND rowhead[i].tag = geq AND a[i][1] >= 0.0
	                           AND a[i][l] < 0.0 AND a[i][1] - a[i][l] * a[k][1] / a[k][l] < 0.0
	                THEN k _ i
	              ENDLOOP};
	       
               {temp:TableauLabel _ colhead[l]; colhead[l] _ rowhead[k]; rowhead[k] _ temp}; 
               pp _ 1.0 / a[k][l]; a[k][l] _ 1.0;
               -- divide everything in pivot row by the pivot element:
               FOR j IN [1..m] DO a[k][j] _ a[k][j] * pp ENDLOOP;
     
               FOR i IN [1..n] DO 
                          IF i # k THEN 
	           FOR j IN [1..m] DO 
	             IF j # l THEN -- for each a[i,j] outside the pivot row and column
	               a[i][j] _ a[i][j] - a[i][l] * a[k][j]; -- note that a[k,j] was already * pp.
               ENDLOOP ENDLOOP;
      
               -- Finally process pivot column:
               FOR i IN [1..n] DO IF i # k THEN a[i][l] _ -a[i][l] * pp ENDLOOP;
  	          }     

    ENDLOOP;
    
       -- now move the points:
       
    FOR i IN [1..n] DO
      IF rowhead[i].tag IN [x .. y]  
       -- as it will be except for redundant rows
       -- (or unsatisfiable rows)
       THEN
         IF rowhead[i].tag = x
           THEN rowhead[i].point.x _ rowhead[i].point.x + a[i][1]
	   ELSE rowhead[i].point.y _ rowhead[i].point.y + a[i][1];
    ENDLOOP;
                               
      END;
END; 
 
   
END.