MODULE Curve;

(* Define curves by sequences of points. *)

IMPORT List, R2, Bezier, Geometry, PS;

/* The implementation of the "Fit" procedure was 
   taken from the book "Graphic Gems", edited by 
   Andrew S. Glassner and published by AP 
   Professional, New York, 1990. The algorithm is 
   described in the section "An Algorithm for 
   Automatically Fitting Digitized Curves" by Philip 
   J. Schneider, pp. 612-626. */

CONST DefaultErrorBound = 20;

(* This module maintains a current error bound, 
   which is used by the "Fit" (and hence the 
   "EndPoints") procedures below. *)

PRIVATE VAR errorBound := DefaultErrorBound;

PRIVATE CONST MinErrorBound = 1;

/* A lower bound on the minimum allowed error bound. 
*/

PROC SetErrorBound(errBnd) IS 
  IF 
    errBnd < MinErrorBound -> errBnd := MinErrorBound 
  | SKIP 
  FI; 
  errorBound := errBnd 
END;

UI SetTool(SetErrorBound);

(* Set the current error bound to "errBnd". *)

UI Param(SetErrorBound, DefaultErrorBound);

UI Param(SetErrorBound, 5);

UI Param(SetErrorBound, 10);

UI Param(SetErrorBound, 20);

UI Param(SetErrorBound, 50);

UI Param(SetErrorBound, 100);

UI Param(SetErrorBound, 200);

UI Param(SetErrorBound, 500);

PRIVATE CONST MaxIterations = 1, False = 0, True = 1;

/* MISCELLANEOUS FUNCTIONS */

PRIVATE FUNC ie = IterationError(error) IS 
  ie = 5 * error 
END;

PRIVATE PROC v:Inc(x) IS v := v + x END;

PRIVATE FUNC res = Sum3(a, b, c) IS 
  res = R2.Plus(a, R2.Plus(b, c)) 
END;

PRIVATE FUNC res = Sum4(a, b, c, d) IS 
  res = R2.Plus(a, R2.Plus(b, R2.Plus(c, d))) 
END;

PRIVATE PROC res := B0(t) IS 
  VAR x IN x := 1 - t; res := x * x * x END 
END;

PRIVATE PROC res := B1(t) IS 
  VAR x IN x := 1 - t; res := 3 * t * x * x END 
END;

PRIVATE PROC res := B2(t) IS 
  res := 3 * t * t * (1 - t) 
END;

PRIVATE PROC res := B3(t) IS res := t * t * t END;

/* ROUTINES FOR OPTIMIZING TIMESTAMPS */

PRIVATE PROC res := ImproveTimeStamp(pt, bez, ts) IS 
  IF 
    VAR a, b, c, d, p1, p2, p3 IN 
      bez = [a, b, c, d] AND 
      Bezier.MapRep(a, b, c, d, p1, p2, p3) -> 
        IF 
          VAR tsPrime ~ ts IN 
            0 = 
              R2.Dot(R2.Minus(pt, 
                              Bezier.AtTFromCoeffs(a, 
                                                   p1, 
                                                   p2, 
                                                   p3, 
                                                   tsPrime)), 
                     Bezier.PrimeAtTFromCoeffs(a, 
                                               p1, 
                                               p2, 
                                               p3, 
                                               tsPrime)) -> 
              res := tsPrime 
          END 
        FI 
    END 
  FI 
END;

PRIVATE PROC res := Reparameterize(pts, ts, bez) IS 
  res := NIL; 
  DO 
    pts # NIL -> 
      res := 
        (ImproveTimeStamp(CAR(pts), bez, CAR(ts)), 
         res); 
      pts, ts := CDR(pts), CDR(ts) 
  OD; 
  res := List.Reverse(res) 
END;

/* ROUTINES FOR DOING RECURSIVE SPLIT */

PRIVATE PROC 
  res := ComputeCenterTangent(pts, middle) IS 
  pts := List.SuffixFrom(pts, middle - 1); 
  res := 
    R2.Normalize(R2.Minus(CAR(pts), List.Nth(pts, 2))) 
END;

/* Requires "3 <= List.Length(pts)" and "1 <= middle 
   < List.Length(pts) -1" */

PRIVATE PROC l, r := Split(pts, middle) IS 
  l := List.Prefix(pts, middle + 1); 
  r := List.SuffixFrom(pts, middle) 
END;

/* BASIC ROUTINES */

PRIVATE PROC AppendBezier(bez) IS 
  IF 
    VAR p0, p1, p2, p3 IN 
      bez = [p0, p1, p2, p3] -> 
        PS.CurveTo(p1, p2, p3) 
    END 
  FI 
END;

/* Requires "2 <= List.Size(pts)" */

PRIVATE PROC 
  bez := BezHeuristic(pts, tHat1, tHat2) IS 
  VAR dist IN 
    dist := 
      Geometry.Dist(CAR(pts), List.Last(pts)) / 3; 
    bez := BezCurve(pts, tHat1, tHat2, dist, dist) 
  END 
END;

/* Requires "2 <= List.Size(pts)" */

PRIVATE PROC ts := ChordLengthTimeStamps(pts) IS 
  VAR dists = [0], prev IN 
    prev, pts := CAR(pts), CDR(pts); 
    DO 
      pts # NIL -> 
        dists := 
          (CAR(dists) + Geometry.Dist(prev, CAR(pts)), 
           dists); 
        prev, pts := CAR(pts), CDR(pts) 
    OD; 
    VAR sum IN 
      sum := CAR(dists); 
      ts := NIL; 
      DO 
        dists # NIL -> 
          ts := (CAR(dists) / sum, ts); 
          dists := CDR(dists) 
      OD 
    END 
  END 
END;

/* Set "ts" to a list of the approximate timestamps 
   for the list points "pts". The timestamps are 
   monotonically increasing in the interval [0, 1]. 
*/

PRIVATE PROC 
  a1, a2 := ComputeMagnitudesA(ts, tHat1, tHat2) IS 
  a1, a2, ts := NIL, NIL, List.Reverse(ts); 
  DO 
    ts # NIL -> 
      a1 := (R2.Times(B1(CAR(ts)), tHat1), a1); 
      a2 := (R2.Times(B2(CAR(ts)), tHat2), a2); 
      ts := CDR(ts) 
  OD 
END;

PRIVATE PROC 
  c00, c01, c10, c11, x0, x1 := 
    ComputeMagnitudesCX(pts, ts, tHat1, tHat2) IS 
  VAR a1, a2, ptFirst, ptLast IN 
    a1, a2 := ComputeMagnitudesA(ts, tHat1, tHat2); 
    ptFirst, ptLast := CAR(pts), List.Last(pts); 
    c00, c01, c10, c11, x0, x1 := 0, 0, 0, 0, 0, 0; 
    DO 
      pts # NIL -> 
        c00:Inc(R2.Dot(CAR(a1), CAR(a1))); 
        VAR k IN 
          k := R2.Dot(CAR(a1), CAR(a2)); 
          c01:Inc(k); 
          c10:Inc(k) 
        END; 
        c11:Inc(R2.Dot(CAR(a2), CAR(a2))); 
        VAR v IN 
          v := 
            R2.Minus(CAR(pts), 
                     Sum4(R2.Times(B0(CAR(ts)), 
                                   ptFirst), 
                          R2.Times(B1(CAR(ts)), 
                                   ptFirst), 
                          R2.Times(B2(CAR(ts)), 
                                   ptLast), 
                          R2.Times(B3(CAR(ts)), 
                                   ptLast))); 
          x0:Inc(R2.Dot(CAR(a1), v)); 
          x1:Inc(R2.Dot(CAR(a2), v)) 
        END; 
        pts, ts := CDR(pts), CDR(ts); 
        a1, a2 := CDR(a1), CDR(a2) 
    OD 
  END 
END;

PRIVATE PROC 
  detc0c1, detc0x, detxc1 := 
    ComputeDeterminants(pts, ts, tHat1, tHat2) IS 
  VAR c00, c01, c10, c11, x0, x1 IN 
    c00, c01, c10, c11, x0, x1 := 
      ComputeMagnitudesCX(pts, ts, tHat1, tHat2); 
    detc0c1 := c00 * c11 - c10 * c01; 
    detc0x := c00 * x1 - c01 * x0; 
    detxc1 := x0 * c11 - x1 * c01; 
    IF 
      detc0c1 = 0 -> detc0c1 := c00 * c11 * 1e-11 
    | SKIP 
    FI 
  END 
END;

PRIVATE PROC 
  l, r := ComputeMagnitudes(pts, ts, tHat1, tHat2) IS 
  VAR detc0c1, detc0x, detxc1 IN 
    detc0c1, detc0x, detxc1 := 
      ComputeDeterminants(pts, ts, tHat1, tHat2); 
    l := detxc1 / detc0c1; 
    r := detc0x / detc0c1 
  END 
END;

PRIVATE PROC 
  bez := BezCurve(pts, tHat1, tHat2, d1, d2) IS 
  VAR p0, p3 IN 
    p0, p3 := CAR(pts), List.Last(pts); 
    bez := 
      [p0, R2.Plus(p0, R2.Times(d1, tHat1)), 
       R2.Plus(p3, R2.Times(d2, tHat2)), p3] 
  END 
END;

PRIVATE PROC 
  bez := GenerateBezier(pts, ts, tHat1, tHat2) IS 
  VAR alphaL, alphaR IN 
    alphaL, alphaR := 
      ComputeMagnitudes(pts, ts, tHat1, tHat2); 
    IF 
      alphaL < 0 OR alphaR < 0 -> 
        bez := BezHeuristic(pts, tHat1, tHat2) 
    | bez := 
        BezCurve(pts, tHat1, tHat2, alphaL, alphaR) 
    FI 
  END 
END;

PRIVATE PROC cl := BezClosure(bez) IS 
  IF 
    VAR p0, p1, p2, p3 IN 
      bez = [p0, p1, p2, p3] -> 
        cl := Bezier.Closure(p0, p1, p2, p3) 
    END 
  FI 
END;

PRIVATE PROC 
  maxError, splitPoint := 
    ComputeMaxError(pts, bez, ts) IS 
  VAR cl, dist, i, point IN 
    cl := BezClosure(bez); 
    pts, ts, i := CDR(pts), CDR(ts), 1; 
    maxError := 0; 
    DO 
      CDR(pts) # NIL -> 
        point := APPLY(cl, CAR(ts)); 
        dist := Geometry.Dist2(CAR(pts), point); 
        IF 
          maxError <= dist -> 
            maxError, splitPoint := dist, i | SKIP 
        FI; 
        pts, ts, i := CDR(pts), CDR(ts), i + 1 
    OD 
  END 
END;

/* Requires "3 <= List.Length(pts)" and 
   "List.Length(pts) = List.Length(ts)" */

PRIVATE PROC 
  DrawCubicMany(pts, tHat1, tHat2, error) IS 
  VAR ts, bez, maxError, splitPoint, i, done = False IN 
    ts := ChordLengthTimeStamps(pts); 
    bez := GenerateBezier(pts, ts, tHat1, tHat2); 
    maxError, splitPoint := 
      ComputeMaxError(pts, bez, ts); 
    IF 
      maxError < error -> 
        AppendBezier(bez); done := True 
    | maxError < IterationError(error) -> 
        i := 0; 
        DO 
          done = False AND i < MaxIterations -> 
            ts := Reparameterize(pts, ts, bez); 
            bez := 
              GenerateBezier(pts, ts, tHat1, tHat2); 
            maxError, splitPoint := 
              ComputeMaxError(pts, bez, ts); 
            IF 
              maxError < error -> 
                AppendBezier(bez); done := True 
            | i := i + 1 
            FI 
        OD | SKIP 
    FI; 
    IF 
      done = False -> 
        VAR 
          tHatCenter 
        = ComputeCenterTangent(pts, splitPoint), 
          leftPts, rightPts 
        IN 
          leftPts, rightPts := Split(pts, splitPoint); 
          DrawCubic(leftPts, tHat1, tHatCenter, error); 
          tHatCenter := R2.Times(-1, tHatCenter); 
          DrawCubic(rightPts, tHatCenter, tHat2, 
                    error) 
        END | SKIP 
    FI 
  END 
END;

PRIVATE PROC res := ComputeLeftTangent(pts) IS 
  res := 
    R2.Normalize(R2.Minus(List.Nth(pts, 1), 
                          List.Nth(pts, 0))) 
END;

/* Requires "2 <= List.Size(pts)" */

PRIVATE PROC res := ComputeRightTangent(pts) IS 
  res := ComputeLeftTangent(List.Reverse(pts)) 
END;

/* Requires "2 <= List.Size(pts)" */

PRIVATE PROC DrawCubic(pts, tHat1, tHat2, error) IS 
  IF 
    List.Length(pts) = 2 -> 
      AppendBezier(BezHeuristic(pts, tHat1, tHat2)) 
  | DrawCubicMany(pts, tHat1, tHat2, error) 
  FI 
END;

PROC FitPointList(pts) IS 
  VAR len IN 
    len := List.Length(pts); 
    PS.MoveTo(CAR(pts)); 
    IF 
      len = 1 -> SKIP 
    | len = 2 -> PS.LineTo(CAR(CDR(pts))) 
    | len > 2 -> 
        DrawCubic(pts, ComputeLeftTangent(pts), 
                  ComputeRightTangent(pts), 
                  errorBound) 
    FI 
  END 
END;

(* Augment the current path with a sequence of 
   connected Bezier curves that approximate the 
   points in the nonempty list "pts". The sum of the 
   squared distances of the given points to the 
   constructed curve is at most the current error 
   bound. *)

PRIVATE VAR ptList := NIL;

/* The list of points augmented, used, and reset by 
   the "AddPoint" and "FitPoints" procedures. */

(* This module maintains a list of points, initially 
   empty. Use the "AddPoint" procedure to append a 
   point to the list, and the "FitPoints" procedure 
   to draw a curve through the points. *)

PROC AddPoint(p) IS ptList := (p, ptList) END;

UI PointTool(AddPoint);

(* Add the point "p" to the current list of points. 
*)

PROC FitPoints() IS 
  FitPointList(List.Reverse(ptList)); ptList := NIL 
END;

UI PointTool(FitPoints);

(* Equivalent to "FitPointList(l)", where "l" is the 
   current list of points. On exit, the current list 
   of points is reset to the empty list. *)

PRIVATE VAR history := NIL;

PROC Save() IS 
  history := ((errorBound, ptList), history) 
END;

PROC Restore() IS 
  IF 
    VAR hd, tl IN 
      history = (hd, tl) -> 
        errorBound := CAR(hd); 
        ptList := CDR(hd); 
        history := tl 
    END 
  FI 
END;

UI PointTool(Save);

UI PointTool(Restore);

(* Save/restore the current error bound and point 
   list. *)