(load "package.lisp")
(load "bezier.lisp")
(in-package :so3-cnc)

(ql:quickload :alexandria)
(ql:quickload :cl-ppcre)
(ql:quickload :jsown)
(ql:quickload :parse-float)
(ql:quickload :s-http-client)

; width of the work area, in stepper steps
(setf *width* 0)
; height of the work area, in stepper steps
(setf *height* 0)

(setf *steps-per-mm* 50)

; these are the states that cncserver recognizes
(setq *pen-states* '("up" "draw"))

(defun api-url (e) (format nil "http://saito:4242/~a" e))

(defun extract (d v) (cdr (assoc v d :test #'equal)))

(defun init ()
  (let* ((settings (jsown:parse (s-http-client:do-http-request (api-url "v1/settings/bot"))))
         (max-area (jsown:val settings "maxArea")))
    (setf *width* (parse-integer (jsown:val max-area "width")))
    (setf *height* (parse-integer (jsown:val max-area "height")))
    (send-put-request (api-url "v1/settings/global")
                      (jsown:new-js ("invertAxis" (jsown:new-js ("x" :f) ("y" :f)))))
    settings
    ))

(defun rel-x (x) (when (= *width* 0) (init)) (* 100 (/ x *width*)))
(defun rel-y (y) (when (= *width* 0) (init)) (* 100 (/ y *height*)))
(defun abs-x (x) (when (= *width* 0) (init)) (/ (* x *width*) 100))
(defun abs-y (y) (when (= *width* 0) (init)) (/ (* y *height*) 100))

(defun get-pen-raw () (cdr (jsown:parse (s-http-client:do-http-request (api-url "v1/pen")))))
(defun get-pen ()
  (let* ((pen-data (get-pen-raw))
         (extract-xyz
           (lambda (d) (list (extract d "x")
                             (extract d "y")
                             (position (extract d "state") *pen-states* :test #'equal)))))
    (funcall extract-xyz pen-data)))

(defun send-put-request (url js)
  (s-http-client:do-http-request
    url :method :PUT :content (jsown:to-json js) :content-type "application/json; charset=UTF-8"))

(defun set-pen-rel (x y z &key (wait-time 0))
  (send-put-request (api-url "v1/pen") (jsown:new-js ("state" (nth z *pen-states*))))
  (send-put-request (api-url "v1/pen") (jsown:new-js ("x" x) ("y" y))))

(defun disable-motors ()
  (s-http-client:do-http-request (api-url "v1/motors") :method :DELETE))

(defun set-zero ()
  (send-put-request (api-url "v1/motors") (jsown:new-js ("reset" 1))))

(defun start-buffer ()
  (send-put-request (api-url "v1/buffer") (jsown:new-js ("paused" t))))
(defun flush-buffer ()
  (send-put-request (api-url "v1/buffer") (jsown:new-js ("paused" :f))))

(defun split-by-one-space (string)
  (loop for i = 0 then (1+ j)
        as j = (position #\Space string :start i)
        collect (subseq string i j)
        while j))

(defun gcode-remove-comments (line)
  (if (equal (char line 0) #\#) ""
      (string-trim '(#\Space #\Newline #\Linefeed #\Return)
                   (cl-ppcre:regex-replace-all "\\([^)]*\\)" line ""))))

(defun gcode-assign-parse (dirty-line)
  (let ((line (gcode-remove-comments dirty-line)))
    (if (= (length line) 0)
        nil
        (remove-if
         (lambda (pair) (null (car pair)))
         (mapcar (lambda (term)
                   (let* ((register (char term 0))
                          (reg (cond
                                 ((equal register #\G) :G)
                                 ((equal register #\X) :X)
                                 ((equal register #\Y) :Y)
                                 ((equal register #\Z) :Z)
                                 ((equal register #\I) :I)
                                 ((equal register #\J) :J)
                                 (t nil)
                                 )))
                     (if (null reg) (cons nil nil)
                         (cons reg
                               (funcall (if (eq reg :G) #'parse-integer #'parse-float:parse-float)
                                        (subseq term 1))))))
                 (split-by-one-space (gcode-remove-comments line)))))))

(defstruct (gvm (:conc-name gvm-))
  (mode 1 :type integer)
  (i 0.0 :type float)
  (j 0.0 :type float)
  (x 0.0 :type float)
  (y 0.0 :type float)
  (z "unknown" :type string)
  (dummy-file nil)
  )

(defun x-from-gcode (x-mm) (- 100 (rel-x (* x-mm *steps-per-mm*))))
(defun y-from-gcode (y-mm) (rel-y (* y-mm *steps-per-mm*)))

(defun theta-steps-ccw (start end)
  (cond
    ((< end start) (theta-steps-ccw start (+ end tau)))
    ; if start and end are the same, we're making a full circle.
    ((< (abs (- end start)) 0.0001) (theta-steps-ccw start (+ end tau)))
    (t (loop for theta from start to end
             by (min (/ (- end start) 4) (* tau (/ 15 360)))
             collect theta))))

(defun theta-steps-cw (start end)
  (reverse (theta-steps-ccw end start)))

(defun theta-steps (mode start end)
  (funcall (alexandria:switch (mode)
             (2 #'theta-steps-cw)
             (3 #'theta-steps-ccw)) start end))

(defun run-gcode-circle (g x y i j old-x old-y)
  (let* ((cx (+ old-x i))
         (cy (+ old-y j))
         (r (sqrt (+ (expt i 2) (expt j 2))))
         (r-end (sqrt (+ (expt (- x cx) 2) (expt (- y cy) 2))))
         (ni (- cx x))
         (nj (- cy y))
         (theta-end (atan (- nj) (- ni)))
         (theta-start (atan (- j) (- i)))
         )
    (unless (> (abs (- r-end r)) 0.001)
      (loop for theta in (theta-steps (gvm-mode g) theta-start theta-end)
            do (let ((tx (+ cx (* r (cos theta))))
                          (ty (+ cy (* r (sin theta)))))
                 (run-gcode-linear g tx ty)))
      (run-gcode-linear g x y))))

(defun run-gcode-linear (g x y)
  (if (null (gvm-dummy-file g))
      (send-put-request
       (api-url "v1/pen")
       (jsown:new-js ("x" (x-from-gcode x)) ("y" (y-from-gcode y)) ("ignoreTimeout" 1)))
      (format (gvm-dummy-file g) "~f,~f~%" (x-from-gcode x) (y-from-gcode y))
      ))

(defun run-gcode-movement (g old-x old-y)
  (alexandria:switch ((gvm-mode g))
    (0 (run-gcode-linear g (gvm-x g) (gvm-y g)))
    (1 (run-gcode-linear g (gvm-x g) (gvm-y g)))
    (2 (run-gcode-circle g (gvm-x g) (gvm-y g) (gvm-i g) (gvm-j g) old-x old-y))
    (3 (run-gcode-circle g (gvm-x g) (gvm-y g) (gvm-i g) (gvm-j g) old-x old-y))
    ))

(defun update-state-from-gcode (g state)
  (let ((new-state-name (nth (if (> state 0) 0 1) *pen-states*)))
    (unless (or (not (null (gvm-dummy-file g))) (eq new-state-name (gvm-z g)))
      (send-put-request (api-url "v1/pen") (jsown:new-js ("state" new-state-name)))
      )))

(defun run-gcode-line (line g)
  (let* ((terms (gcode-assign-parse line))
         (new-mode (assoc :G terms))
         (new-state (assoc :Z terms))
         (new-x (assoc :X terms))
         (new-y (assoc :Y terms))
         (new-i (assoc :I terms))
         (new-j (assoc :J terms))
         (old-x (gvm-x g))
         (old-y (gvm-y g)))
    (unless (null new-mode) (setf (gvm-mode g) (cdr new-mode)))
    (unless (null new-state) (update-state-from-gcode g (cdr new-state)))
    (unless (null new-x) (setf (gvm-x g) (cdr new-x)))
    (unless (null new-y) (setf (gvm-y g) (cdr new-y)))
    (unless (null new-i) (setf (gvm-i g) (cdr new-i)))
    (unless (null new-j) (setf (gvm-j g) (cdr new-j)))
    (run-gcode-movement g old-x old-y)))

(defun run-gcode-file-on-gvm (g fname)
  (with-open-file (stream fname :direction :input)
    (do ((l (read-line stream) (read-line stream nil 'eof)))
        ((eq l 'eof) nil)
      (run-gcode-line l g))))

(defun run-gcode-file (file &key (dummy-file nil))
  (if (null dummy-file) (run-gcode-file-on-gvm (make-gvm) file)
      (with-open-file (output dummy-file :direction :output :if-exists :supersede)
        (run-gcode-file-on-gvm (make-gvm :dummy-file output) file))
      ))