;;;; utils.lisp

(in-package #:wheelwork)

(declaim (inline radians counterclockwisep points-equal-p))
(defun radians (degrees)
  "Converse DEGREES to radians"
  (* degrees  0.017453292519943295d0))

(defun safe-slot (object slot &optional default)
  (if-let (val (and (slot-exists-p object slot)
                    (slot-boundp object slot)
                    (slot-value object slot)))
    val
    default))

(defun counterclockwisep (ax ay  bx by cx cy)
  "A B and C are vectors created by 3d-vectors:vec, each representing
a 2d point.  Returns T if the three are supplied in counterclockwise
order, nil if not."
  (declare (type single-float ax ay bx by cx cy))
  (> (* (- bx ax)
        (- cy ay))
     (* (- by ay)
        (- cx ax))))

(defun points-equal-p (x1 y1 x2 y2)
  (declare (type single-float x1 x2 y1 y2))
  (and (= x1 x2)) (= y1 y2))

(defun segments-intersect-p (ax ay bx by cx cy dx dy)
  "A B C and D are vectors of the sort created by 3d-vectors:vec,
each representing a 2d point.  Returns T if the line segment between A
and B intersects the linesegment between C and D, NIL otherwise."
  (declare (optimize (speed 3) (safety 0)))
  (declare (type single-float ax ay bx by cx cy dx dy))
  (or (points-equal-p ax ay cx cy)
      (points-equal-p ax ay dx dy)
      (points-equal-p bx by cx cy)
      (points-equal-p bx by dx dy)
      (and (not (eq (counterclockwisep ax ay cx cy dx dy)
                    (counterclockwisep bx by cx cy dx dy)))
           (not (eq (counterclockwisep ax ay bx by cx cy)
                    (counterclockwisep ax ay bx by dx dy))))))

(defun paths-intersect-p (path1 path2)
  "Paths are lists of vectors, each of which represents a 2d point."
  (declare (optimize (speed 3) (safety 0) ))
  (loop for ((ax ay) (bx by) . more1) on path1
        while bx
          thereis (loop for ((cx cy) (dx dy) . more2) on path2
                        while dx
                          thereis (segments-intersect-p ax ay bx by cx cy dx dy))))

(defun closed-path-p (path)
  (equalp (first path)
          (first (last path))))

(defun path-encloses-point-p (path px py)
  "Path is a list of points, pt is a single vector."
  (assert (closed-path-p path) () "Enclosing path must be a closed path.")
  (let* ((bounds
           (path-bounds path))
         (corner
           ;; creating a point guaranteed to be outside of the path
           (list (- (getf bounds :left) (getf bounds :width))
                 (- (getf bounds :bottom) (getf bounds :height)))))
    (loop
      with (cx cy) = corner
      for ((ax ay) (bx by) . more) on path
      while bx
      when (segments-intersect-p ax ay bx by px py cx cy)
        count 1 into intersection-count
      finally
         (return (oddp intersection-count)))))

;; (defun path-encloses-path-p (path-a path-b)
;;   "T if path-b is totally contained in path-a and does not intersect path-a"
;;   (assert (closed-path-p path-a) () "Enclosing path must be a closed path.")
;;   (and 
;;    (loop for (p1 p2 . more) on path-b
;;          while p2
;;          always (path-encloses-point-p path-a p1))
;;    (not (paths-intersect-p path-a path-b))))

(defun path-bounds (path)
  "Path is a list of vectors representing 2d points. Returns the
bounds and width and height as a plist of the form 

(:top N :left N :right N :bottom N :width N :height N)

This is the smallest UNROTATED RECTANGLE that contains the points in
the path."
  (loop
    with max-x = nil
    and max-y = nil
    and min-x = nil
    and min-y = nil
    for (x y) in path
    when (or (null max-x) (< max-x x))
      do (setf max-x x)
    when (or (null min-x) (< x min-x))
      do (setf min-x x)
    when (or (null max-y) (< max-y y))
      do (setf max-y y)
    when (or (null min-y) (< y min-y))
      do (setf min-y y)
    finally
       (return (list :top max-y :left min-x :right max-x :bottom min-y
                     :width (- max-x min-x)
                     :height (- max-y min-y)))))

(defmacro setf-many (&rest places-and-value)
  "e.g. (setf-many a b c 10) would set a b and c to 10"
  (let* ((value-form
           (first (last places-and-value)))
         (value
           (gensym))
         (clauses
           (loop for place in (butlast places-and-value)
                 append `(,place ,value))))
    `(let ((,value ,value-form))
       (setf ,@clauses))))

(defun euclidean-dist (x1 y1 x2 y2)
  (let ((dx (- x2 x1))
        (dy (- y2 y1)))
    (sqrt (+ (* dx dx) (* dy dy)))))

(let ((cache
        (make-array 100 :adjustable t :initial-element nil)))
 (defun factorial (n)
   (cond
     ((zerop n) 1)
     ((< n (length cache))
      (or (aref cache n)
          (setf (aref cache n)
                (* n (factorial (1- n))))))
     ((>= n (length cache))
      (setf cache (adjust-array cache (* 2 (length cache))))
      (factorial n)))))

(defun binomial-coefficient (n k)
  (/ (factorial n)
     (* (factorial k) (factorial (- n k)))))

(defun bezier-lambda (&rest points)
  (let* ((n
           (1- (length points)))
         (bin-coeffs
           (loop for i from 0 to n collect (binomial-coefficient n i))))
    (lambda (a)
      (loop for (x y) in points
            for i from 0
            for bin-coeff in bin-coeffs
            for coeff = (* bin-coeff
                           (expt (- 1 a) (- n i))
                           (expt a i))
            sum (* coeff x) into bx
            sum (* coeff y) into by
            finally (return (list bx by))))))

(defun clamp (lo val hi)
  "Returns VAL if (< LO VAL HI), otherwise returns LO or HI depending
on which boundary VAL is outside of."
  (max lo (min val hi)))