(* This module contains a test of Newton-Raphson 
   iteration similar to the one described by 
   Glenn Kramer in his book "Solving Geometric 
   Constraint Systems", section 7.2.2, pgs 
   155-157. *)

PRIVATE CONST 
  LenA = 400, 
  LenB = 600, 
  NumRows = 10, 
  NumCols = NumRows, 
  StartAVal = -Math.Pi, 
  EndAVal = Math.Pi, 
  StartBVal = StartAVal, 
  EndBVal = EndAVal, 
  DeltaA = 2 * Math.Pi / (NumRows - 1), 
  DeltaB = 2 * Math.Pi / (NumCols - 1);

PRIVATE CONST 
  HSep = 500, 
  A1X = -250, 
  A1 = (A1X, 0), 
  B1 = (A1X + HSep, 0);

FUNC res = AnglePt(center, theta, len) IS 
  res = 
    R2.Plus(center, 
            R2.Times(len, (COS(theta), SIN(theta)))) 
END;

(* "res" is the point a distance "len" from the 
   point "center" rotated "theta" radians from 
   the horizontal ray to the right in a 
   counter-clockwise direction. *)

CONST TwoBars = TwoBars2;

(* Simulate the two bars system with horizontal 
   base points "A1", "B1" and initial bar angles 
   "alpha" and "beta" with bar lengths "lenA" and 
   "lenB". Return +1 if the solver converges on 
   the upper solution, -1 if the solver converges 
   on the lower solution, and 0 if it fails to 
   converge. *)

PRIVATE PROC 
  res := (alpha, beta):GoodTwoBars() IS 
  IF 
    VAR 
      a2 ~ AnglePt(A1, alpha, LenA), 
      b2 ~ AnglePt(B1, Math.Pi - beta, LenB) 
    IN 
      Geometry.Dist(A1, a2) = LenA AND 
      Geometry.Dist(B1, b2) = LenB AND 
      a2 = b2 -> 
        IF 
          CDR(a2) > CDR(A1) -> res := 1 
        | res := -1 
        FI; 
        VAR delta IN 
          delta := R2.Minus(a2, A1); 
          alpha := ATAN(CDR(delta), CAR(delta)); 
          delta := R2.Minus(b2, B1); 
          beta := 
            Math.Pi - 
            (ATAN(CDR(delta), CAR(delta)) MOD 
             (2 * Math.Pi)) 
        END 
    END | res := 0 
  FI 
END;

PRIVATE PROC res := TwoBars0(alpha, beta) IS 
  res := (alpha, beta):GoodTwoBars() 
END;

PRIVATE PROC res := TwoBars1(alpha, beta) IS 
  IF 
    VAR 
      a2 ~ AnglePt(A1, alpha, LenA), 
      b2 ~ AnglePt(B1, Math.Pi - beta, LenB) 
    IN 
      Geometry.Dist(A1, a2) = LenA AND 
      Geometry.Dist(B1, b2) = LenB AND 
      Geometry.Dist(a2, b2) = 0 -> 
        IF 
          CDR(a2) > CDR(A1) -> res := 1 
        | res := -1 
        FI 
    END | res := 0 
  FI 
END;

PRIVATE PROC res := TwoBars2(alpha, beta) IS 
  IF 
    VAR a2 ~ alpha, b2 ~ Math.Pi - beta IN 
      LenA * COS(a2) - (HSep + LenB * COS(b2)) = 0 AND 
      LenA * SIN(a2) - LenB * SIN(b2) = 0 -> 
        IF a2 > 0 -> res := 1 | res := -1 FI 
    END | res := 0 
  FI 
END;

CONST DotSize = 4;

PROC DrawDot(a) IS 
  Circle.Draw(a, R2.Plus(a, (DotSize, 0))) 
END;

PROC res := Interpolate(p, q, alpha, beta) IS 
  VAR delta = R2.Minus(q, p), fx, fy IN 
    fy := 
      (alpha - StartAVal) / (EndAVal - StartAVal); 
    fx := 
      (beta - StartBVal) / (EndBVal - StartBVal); 
    res := 
      R2.Plus(p, 
              (fx * CAR(delta), fy * CDR(delta))) 
  END 
END;

PROC DrawSoln(p, q, theta, c) IS 
  SAVE PS IN 
    VAR alpha, beta, res, a IN 
      alpha, beta := theta, theta; 
      res := (alpha, beta):GoodTwoBars(); 
      a := Interpolate(p, q, alpha, beta); 
      DrawDot(a); 
      PS.SetColor(c); 
      PS.Fill() 
    END 
  END 
END;

PROC DrawBlock(p, diag, r, c) IS 
  VAR sw IN 
    sw := 
      R2.Plus(p, (c * CAR(diag), r * CDR(diag))); 
    Rect.Draw(sw, R2.Plus(sw, diag)) 
  END 
END;

PROC TestPoint(p, diag, alpha, beta, r, c) IS 
  VAR res IN 
    res := APPLY(TwoBars, alpha, beta); 
    res := (res + 1) / 2; 
    DrawBlock(p, diag, r, c); 
    PS.SetColor(Color.FromGrey(res)); 
    PS.Fill() 
  END 
END;

PROC TestRow(p, diag, alpha, r) IS 
  VAR beta, c IN 
    c := 0; 
    beta := StartBVal; 
    DO 
      beta <= EndBVal + 0.001 -> 
        TestPoint(p, diag, alpha, beta, r, c); 
        c := c + 1; 
        beta := beta + DeltaB 
    OD; 
    PS.ShowPage() 
  END 
END;

PROC Test(p, q) IS 
  SAVE PS IN 
    VAR alpha, diag, r IN 
      diag := R2.Minus(q, p); 
      diag := 
        (CAR(diag) / NumCols, CDR(diag) / NumRows); 
      r := 0; 
      alpha := StartAVal; 
      DO 
        alpha <= EndAVal + 0.001 -> 
          TestRow(p, diag, alpha, r); 
          r := r + 1; 
          alpha := alpha + DeltaA 
      OD 
    END; 
    DrawSoln(p, q, Math.Pi / 2, Color.Black); 
    DrawSoln(p, q, -Math.Pi / 2, Color.White) 
  END; 
  Rect.Draw(p, q); 
  PS.Stroke() 
END;

UI PointTool(Test);

PROC Cmd0() IS 
  IF 
    VAR a ~ (-296.8937, -363.3163), b ~ (296.8937, 363.3163) IN 
      R2.Origin = Geometry.Mid(a, b) -> Test(a, b) 
    END 
  FI 
END;