opcodes.lisp

(in-package :gcode)

(defclass pass ()
  ((name :initarg :name :initform "" :reader pass-name)
   (current-x :initarg :current-x :initform 0 :accessor pass-current-x)
   (min-x :initarg :min-x :initform MOST-POSITIVE-FIXNUM :accessor pass-min-x)
   (max-x :initarg :max-x :initform 0 :accessor pass-max-x)
   (current-y :initarg :current-y :initform 0 :accessor pass-current-y)
   (min-y :initarg :min-y :initform MOST-POSITIVE-FIXNUM :accessor pass-min-y)
   (max-y :initarg :max-y :initform 0 :accessor pass-max-y)
   (current-z :initarg :current-z :initform 0 :accessor pass-current-z)
   (min-z :initarg :min-z :initform MOST-POSITIVE-FIXNUM :accessor pass-min-z)
   (max-z :initarg :max-z :initform 0 :accessor pass-max-z)
   (moves :initarg :moves :initform nil :accessor pass-raw-moves)))

(defmethod print-object ((pass pass) stream)
  (print-unreadable-object (pass stream :type t :identity nil)
    (with-slots (name
                 current-x min-x max-x
                 current-y min-y max-y
                 current-z min-z max-z) pass
      (format stream "~S current: ~A,~A,~A min: ~A,~A,~A max: ~A,~A,~A"
              name current-x current-y current-z
              min-x min-y min-z
              max-x max-y max-z))))

(defmethod pass-moves ((pass pass))
  (reverse (pass-raw-moves pass)))

(defmethod (setf pass-moves) (list (pass pass))
  (setf (pass-raw-moves pass) (reverse list)))

(defvar *current-pass*)

(defun current-x ()
  (pass-current-x *current-pass*))
(defun orig-current-x ()
  (first (transform-vector (pass-current-x *current-pass*) (pass-current-y *current-pass*)
                           (invert-matrix *current-transform*))))
(defun (setf current-x) (val)
  (setf (pass-current-x *current-pass*) val))
(defun min-x ()
  (pass-min-x *current-pass*))
(defun (setf min-x) (val)
  (setf (pass-min-x *current-pass*) val))
(defun max-x ()
  (pass-max-x *current-pass*))
(defun (setf max-x) (val)
  (setf (pass-max-x *current-pass*) val))

(defun current-y ()
  (pass-current-y *current-pass*))
(defun orig-current-y ()
  (second (transform-vector (pass-current-x *current-pass*) (pass-current-y *current-pass*)
                            (invert-matrix *current-transform*))))
(defun (setf current-y) (val)
  (setf (pass-current-y *current-pass*) val))
(defun min-y ()
  (pass-min-y *current-pass*))
(defun (setf min-y) (val)
  (setf (pass-min-y *current-pass*) val))
(defun max-y ()
  (pass-max-y *current-pass*))
(defun (setf max-y) (val)
  (setf (pass-max-y *current-pass*) val))

(defun current-z ()
  (pass-current-z *current-pass*))
(defun (setf current-z) (val)
  (setf (pass-current-z *current-pass*) val))
(defun min-z ()
  (pass-min-z *current-pass*))
(defun (setf min-z) (val)
  (setf (pass-min-z *current-pass*) val))
(defun max-z ()
  (pass-max-z *current-pass*))
(defun (setf max-z) (val)
  (setf (pass-max-z *current-pass*) val))

(defmacro with-new-pass ((name) &rest body)
  `(let ((*current-pass* (make-instance 'pass :name ,name)))
     ,@body))

(defun find-named-pass (name)
  (find name (current-passes) :key #'pass-name :test #'string-equal))

(defmacro with-named-pass ((name) &rest body)
  (let ((pass (gensym)))
    `(let ((,pass (find-named-pass ,name)))
       (when (null ,pass)
         (setf ,pass (make-instance 'pass :name ,name))
         (push ,pass (current-passes)))
       (let ((*current-pass* ,pass))
         ,@body))))


(defclass gcode-program ()
  ((passes :initarg :passes :initform nil :accessor gcode-program-passes)
   (name :initarg :name :initform "" :accessor gcode-program-name)))

(defmethod print-object ((program gcode-program) stream)
  (print-unreadable-object (program stream :type t)
    (with-slots (name passes) program
      (format stream "~S~%  passes:~%    ~A" name passes))))

(defun program-min-x (&optional (program *current-program*))
  (let ((passes (gcode-program-passes program)))
    (reduce #'min (mapcar #'pass-min-x passes))))

(defun program-min-y (&optional (program *current-program*))
  (let ((passes (gcode-program-passes program)))
    (reduce #'min (mapcar #'pass-min-y passes))))

(defun program-min-z (&optional (program *current-program*))
  (let ((passes (gcode-program-passes program)))
    (reduce #'min (mapcar #'pass-min-z passes))))

(defun program-max-x (&optional (program *current-program*))
  (let ((passes (gcode-program-passes program)))
    (reduce #'max (mapcar #'pass-max-x passes))))

(defun program-max-y (&optional (program *current-program*))
  (let ((passes (gcode-program-passes program)))
    (reduce #'max (mapcar #'pass-max-y passes))))

(defun program-max-z (&optional (program *current-program*))
  (let ((passes (gcode-program-passes program)))
    (reduce #'max (mapcar #'pass-max-z passes))))

(defun program-width (&optional (program *current-program*))
  (- (program-max-x program) (program-min-x program)))

(defun program-height (&optional (program *current-program*))
  (- (program-max-y program) (program-min-y program)))


(defvar *current-program*)

(defmacro with-program ((name) &rest body)
  `(let ((*current-program* (make-instance 'gcode-program :name ,name))
         (*current-transform* *unity-matrix*))
     (with-named-pass ("mill")
       ,@body
       *current-program*)))

(defun current-passes ()
  (gcode-program-passes *current-program*))

(defun (setf current-passes) (val)
  (setf (gcode-program-passes *current-program*) val))

(defmethod program-pass ((program gcode-program) name)
  (find name (gcode-program-passes program) :key #'pass-name :test #'string-equal))

(defun pass-file-name (file pass)
  (make-pathname :defaults file :name (format nil "~A-~A" (pathname-name file) (pass-name pass))))

(defmethod program-to-file ((program gcode-program) file &key order)
  (unless order
    (setf order (mapcar #'pass-name (gcode-program-passes program))))
  (unless (pathname-name file)
    (setf file (make-pathname :defaults file :name (gcode-program-name program)
                              :type "nc")))
  (let ((all-moves (loop for name in order
                      for pass = (program-pass program name)
                      for moves = (when pass (cons `(:m03) (pass-moves pass)))
                      for pass-filename = (when pass (pass-file-name file pass))
                      collect moves)))
    (g-to-file all-moves file)
    (format t "saved ~A to ~A~%" order file))
  (let ((all-passes (loop for pass in (gcode-program-passes program)
                       for moves = (cons `(:m03) (pass-moves pass))
                       for pass-filename = (pass-file-name file pass)
                       do (g-to-file moves pass-filename)
                       do (format t "saved ~A to ~A~%" (pass-name pass) pass-filename)
                       collect moves)))))



(defmethod program-pass-moves ((program gcode-program) name)
  (pass-moves (program-pass program name)))


;; coordinate variables
#|
(defvar *current-x* 0)
(defvar *current-y* 0)
(defvar *current-z* 0)

(defvar *max-x* 0)
(defvar *max-y* 0)
(defvar *max-z* 0)

(defvar *min-x* MOST-POSITIVE-FIXNUM)
(defvar *min-y* MOST-POSITIVE-FIXNUM)
(defvar *min-z* MOST-POSITIVE-FIXNUM)
|#

;; tool definition

(defclass tool ()
  ((diameter :initarg :diameter :initform 1 :reader tool-diameter) ;; mm
   (feed-xy :initarg :feed-xy :initform 10 :reader tool-feed-xy) ;; mm / min
   (feed-z :initarg :feed-z :initform 5 :reader tool-feed-z) ;; mm / min
   (depth :initarg :depth :initform 3 :reader tool-depth) ;; mm
   (number :initarg :number :initform 1 :reader tool-number)))

(defparameter *test-tool*
  (make-instance 'tool
                 :diameter 2
                 :feed-xy 10
                 :feed-z 10
                 :depth 1))

(defparameter *default-tool*
  (make-instance 'tool))

(defvar *current-tool*
  *default-tool*)

(defmacro with-tool ((tool) &rest body)
  `(let ((*current-tool* ,tool))
     (switch-tool (tool-number ,tool))
     ,@body))

(defvar *gcode-program* nil)
(defvar *gcode-log* nil)

(defun add-gcode-move (move)
  (push move (pass-raw-moves *current-pass*))
  nil)

(defun wait (seconds)
  (add-gcode-move `(:g04 (:h ,seconds))))

(defun make-keyword (string)
  (intern (string-upcase string) (find-package :keyword)))

(defun switch-tool (number)
  (add-gcode-move (list (make-keyword (format nil "T~A" number)))))


(defun copy-pass (p1 &optional (name (pass-name p1)))
  (let ((p2 (make-instance 'pass :name name)))
    (loop for slot in '(current-x min-x max-x
                        current-y min-y max-y
                        current-z min-z max-z moves)
       do (setf (slot-value p2 slot)
                (copy-tree (slot-value p1 slot))))
    p2))

(defun merge-programs (p1 p2)
  (with-program ((format nil "merge ~A-~A" (gcode-program-name p1)
                         (gcode-program-name p2)))
    (loop for pass in (gcode-program-passes p1)
       for new-pass = (copy-pass pass (format nil "1-~A" (pass-name pass)))
       do (push new-pass (current-passes)))
    (loop for pass in (gcode-program-passes p2)
       for new-pass = (copy-pass pass (format nil "2-~A" (pass-name pass)))
       do (push new-pass (current-passes)))))

;;; transform movements

;; movement commands

(defun comment (cmt)
  (add-gcode-move `((:|| ,(format nil "(~A)" cmt)))))

(defun update-current-coords (x y z)
  (when x
    (setf (current-x) x))
  (when y
    (setf (current-y) y))
  (when z
    (setf (current-z) z))
  (when (> (current-x) (max-x))
    (setf (max-x) (current-x)))
  (when (> (current-y) (max-y))
    (setf (max-y) (current-y)))
  (when (> (current-z) (max-z))
    (setf (max-z) (current-z)))
  (when (< (current-x) (min-x))
    (setf (min-x) (current-x)))
  (when (< (current-y) (min-y))
    (setf (min-y) (current-y)))
  (when (< (current-z) (min-z))
    (setf (min-z) (current-z)))
  )

(defun goto (&key x y z cmt)
  (let ((vec (transform-vector (or x (orig-current-x))
                               (or y (orig-current-y))
                               *current-transform*)))
    (when *gcode-log*
      (push `(goto ,@(unless (null x) `(:x ,(first vec)))
                   ,@(unless (null y) `(:y ,(second vec)))
                   ,@(unless (null z) `(:z ,z))) *gcode-program*))
    (update-current-coords (first vec) (second vec) z)
    (add-gcode-move `(:G00 ,@(when x `((:x ,(first vec))))
                           ,@(when y `((:y ,(second vec))))
                           ,@(when z `((:z ,z)))
                           ,@(when cmt (comment cmt))))))

(defun mill (&key x y z f cmt)
  (unless f
    (when *current-tool*
      (setf f (tool-feed-xy *current-tool*))))
  (let ((vec (transform-vector (or x (orig-current-x))
                               (or y (orig-current-y))
                               *current-transform*)))
    (when *gcode-log*
      (push `(mill ,@(unless (null x) `(:x ,(first vec)))
                   ,@(unless (null y) `(:y ,(second vec)))
                   ,@(unless (null z) `(:z ,z))
                   ,@(unless (null f) `(:f ,f)))
            *gcode-program*))
    (update-current-coords (first vec) (second vec) z)
    (add-gcode-move `(:G01 ,@(unless (null x) `((:x ,(first vec))))
                           ,@(unless (null y) `((:y ,(second vec))))
                           ,@(unless (null z) `((:z ,z)))
                           ,@(unless (null f) `((:f ,f)))
                           ,@(when cmt (comment cmt))))))

(defun arc-cw (&key x y z i j k f)
  (let ((vec (transform-vector (or x (orig-current-x))
                               (or y (orig-current-y))
                               *current-transform*))
        (vec2 (transform-vector (or i (orig-current-x))
                                (or j (orig-current-y))
                                *current-transform*)))
    (when *gcode-log*
      (push `(arc-cw ,@(unless (null x) `(:x ,(first vec)))
                     ,@(unless (null y) `(:y ,(second vec)))
                     ,@(unless (null z) `(:z ,z))
                     ,@(when i `(:i ,(first vec2)))
                     ,@(when j `(:j ,(second vec2)))
                     ,@(when k `(:k ,k))
                     ,@(unless (null f) `(:f ,f))) *gcode-program*))
    ;; XXX bounding box better
    (update-current-coords (first vec) (second vec) z)
    (add-gcode-move `(:G02 ,@(when x `((:x ,(first vec))))
                           ,@(when y `((:y ,(second vec))))
                           ,@(when z `((:z ,z)))
                           ,@(when i `((:i ,(first vec2))))
                           ,@(when j `((:j ,(second vec2))))
                           ,@(when k `((:k ,k)))
                           ,@(when f `((:f ,f))))

                    )))

(defun arc-ccw (&key x y z i j k f)
  (let ((vec (transform-vector (or x (orig-current-x))
                               (or y (orig-current-y))
                               *current-transform*))
        (vec2 (transform-vector (or i (orig-current-x))
                                (or j (orig-current-y))
                                *current-transform*)))
    (when *gcode-log*
      (push `(arc-ccw ,@(unless (null x) `(:x ,(first vec)))
                      ,@(unless (null y) `(:y ,(second vec)))
                      ,@(unless (null z) `(:z ,z))
                      ,@(when i `(:i ,(first vec2)))
                      ,@(when j `(:j ,(second vec2)))
                      ,@(when k `(:k ,k))
                      ,@(unless (null f) `(:f ,f))) *gcode-program*))
    (update-current-coords (first vec) (second vec) z)
    (add-gcode-move `(:G03 ,@(when x `((:x ,(first vec))))
                           ,@(when y `((:y ,(second vec))))
                           ,@(when z `((:z ,z)))
                           ,@(when i `((:i ,(first vec2))))
                           ,@(when j `((:j ,(second vec2))))
                           ,@(when k `((:k ,k)))
                           ,@(when f `((:f ,f)))
                           ))))

(defun goto-abs (&key x y z)
  (goto :x x :y y :z z))

(defun goto-rel (&key (x 0) (y 0) (z 0))
  (goto-abs :x (+ (orig-current-x) x)
            :y (+ (orig-current-y) y)
            :z (+ (current-z) z)))

(defun mill-abs (&key x y z f)
  (mill :x x :y y :z z :f f))

(defun mill-rel (&key (x 0) (y 0) (z 0) f)
  (mill-abs :x (+ (orig-current-x) x)
            :y (+ (orig-current-y) y)
            :z (+ (current-z) z)
            :f f))

;; arcs

(defun arc-cw-abs (&key x y z i j k f)
  (arc-cw :x x :y y :z z :i i :j j :k k :f f))

(defun arc-cw-rel (&key (x (orig-current-x)) (y (orig-current-y)) (z 0) (i 0) (j 0) (k 0) f)
  (arc-cw-abs :x (+ (orig-current-x) x)
              :y (+ (orig-current-y) y)
              :z (+ (current-z) z)
              :i (+ (orig-current-x) i)
              :j (+ (orig-current-y) j)
              :k (+ (current-z) k)
              :f f))

(defun arc-ccw-abs (&key x y z i j k f)
  (arc-ccw :x x :y y :z z :i i :j j :k k :f f))

(defun arc-ccw-rel (&key x y z i j k f)
  (arc-ccw-abs  :x (+ (orig-current-x) x)
                :y (+ (orig-current-y) y)
                :z (+ (current-z) z)
                :i (+ (orig-current-x) i)
                :j (+ (orig-current-y) j)
                :k (+ (current-z) k)
                :f f))

(defmacro with-rel-back-xy (() &rest body)
  (let ((x (gensym))
        (y (gensym)))
    `(let ((,x (orig-current-x))
           (,y (orig-current-y)))
       ;;       (format t "x before: ~A, y before: ~A~%" ,x ,y)
       ,@body
       (format t "x after ~A, y after: ~A~%" (orig-current-x) (orig-current-y))
       (goto-rel :x (- ,x (orig-current-x))
                 :y (- ,y (orig-current-y))))))


;; CORRECTION is:
;; - middle - just follow outline with tool
;; - outer
;; - inner

(defmacro repeat-for-depth ((depth) &rest body)
  (let ((curx (gensym))
        (cury (gensym)))
    `(let ((nums (/ ,depth (tool-depth *current-tool*))))
       (if (> nums 1)
           (progn
             (loop for i from 0 below nums
                for curdepth from (tool-depth *current-tool*) by (tool-depth *current-tool*)
                do (tool-down :depth (min ,depth curdepth))
                  (let ((,curx (orig-current-x))
                        (,cury (orig-current-y)))
                    ,@body
                    (unless (and (epsilon-= ,curx (orig-current-x))
                                 (epsilon-= ,cury (orig-current-y)))
                      (tool-up)
                      (goto-abs :x ,curx :y ,cury))))
             (tool-up))

           (with-tool-down (,depth)
             ,@body)))))

(defparameter *fly-height* 2.5)

(defun spindle-on ()
  (add-gcode-move `(:m3)))

(defun home ()
  (goto-abs :x 0 :y 0)
  (spindle-on))

(defmacro with-save-xy (() &rest body)
  (let ((oldx (gensym "OLDX"))
        (oldy (gensym "OLDY")))
    `(let ((,oldx (current-x))
           (,oldy (current-y)))
       (unwind-protect
            (progn
              ,@body)
         (setf (current-x) ,oldx
               (current-y) ,oldy)))))

(defmacro with-current-xy ((x y) &rest body)
  `(with-save-xy ()
     (setf (current-x) ,x
           (current-y) ,y)
     ,@body))

(defmacro repeat ((i) &rest body)
  (let ((bla (gensym)))
    `(loop for ,bla from 1 to ,i
        do (progn ,@body))))


;; tool commands

(defun pump-on ()
  (add-gcode-move `(:m8)))

(defun program-stop ()
  (add-gcode-move `(:m00)))

(defun pump-off ()
  (add-gcode-move `(:m9)))

(defun tool-down (&key depth)
  (pump-on)
  (mill-abs :z (if depth
                   (- depth)
                   (- (tool-depth *current-tool*)))
            :f (tool-feed-z *current-tool*)))

(defun tool-up ()
  (goto-abs :z *fly-height*)
  (pump-off))

(defmacro with-tool-down ((&optional depth) &rest body)
  `(progn
     (tool-down :depth ,depth)
     ,@body
     (tool-up)))

(defun fly-to (&key (x (orig-current-x)) (y (orig-current-y)))
  (let ((z (current-z)))
    (unless (and (= (orig-current-x) x)
                 (= (orig-current-y) y))
      (tool-up)
      (goto-abs :x x :y y)
      (tool-down :depth z))))