(load "package.lisp")

(in-package :so3-cnc)

; a more useful constant
(setq tau (* 2 pi))

(defun vec+ (v1 v2) (cons (+ (car v1) (car v2)) (+ (cdr v1) (cdr v2))))
(defun vec- (v1 v2) (cons (- (car v1) (car v2)) (- (cdr v1) (cdr v2))))
(defun vec* (s v) (cons (* s (car v)) (* s (cdr v))))
(defun vec= (v1 v2) (and (= (car v1) (car v2)) (= (cdr v1) (cdr v2))))
(defun dot (v1 v2) (+ (* (car v1) (car v2)) (* (cdr v1) (cdr v2))))
(defun norm (v) (sqrt (dot v v)))
(defun normalize (v) (vec* (/ 1 (norm v)) v))

(defun eval-bezier (theta pts)
  (if (= 1 (length pts))
      (first pts)
      (eval-bezier theta (loop for x in pts
                               for y in (rest pts)
                               collect (vec+ (vec* (- 1 theta) x) (vec* theta y))))))

(defun eval-bezier-cubic (theta p0 p1 p2 p3)
  (eval-bezier theta (list p0 p1 p2 p3)))
(defun eval-bezier-quadratic (theta p0 p1 p2)
  (eval-bezier theta (list p0 p1 p2)))

(defun linspace (a b step)
  (let ((steps (abs (ceiling (/ (- b a) step)))))
    (divspace a b steps)))

(defun divspace (a b steps)
  (let* ((s (/ (- b a) steps)))
    (loop for i from 0 to steps collect (+ a (* s i)))))

(defun linterp-bezier (pts &optional (points-per-mm 0.55))
  (let ((npts (ceiling
               (* points-per-mm
                  (reduce
                   (lambda (sum point-pair)
                     (+ sum (sqrt (+ (expt (- (caar point-pair) (caadr point-pair)) 2)
                                     (expt (- (cdar point-pair) (cdadr point-pair)) 2)))))
                   (mapcar #'list pts (rest pts))
                   :initial-value 0)))))
    (loop for theta in (divspace 0 1 npts) collect (eval-bezier theta pts))))

(defun linterp-bezier-cubic (p0 p1 p2 p3)
  (linterp-bezier (list p0 p1 p2 p3)))
(defun linterp-bezier-quadratic (p0 p1 p2)
  (linterp-bezier (list p0 p1 p2)))

(defun angle-between (v0 v1)
  (* (if (< 0 (- (* (car v0) (cdr v1)) (* (cdr v0) (car v1)))) 1.0d0 -1.0d0)
     (acos (/ (dot v0 v1) (* (norm v0) (norm v1))))
     ))