(proclaim '(optimize (speed 0) (debug 3) (safety 3)))

(defconstant +max-memory+ 1024)

(defconstant +commands+
  '((push . (compile-push #b00000000))
    (pushr . (compile-push #b00001000))
    (pop . (compile-pop #b00000010))
    (popr . (compile-pop #b01000010))
    (loada . (compile-load #b00100010))
    (loadpc . (compile-load #b00101010))
    (loads . (compile-load #b00110010))
    (jump . (compile-jump-call #b00001010))
    (call . (compile-jump-call #b00111010))
    (bcc . (compile-branch #b00000001))
    (bcs . (compile-branch #b01000001))
    (beq . (compile-branch #b10000001))
    (bne . (compile-branch #b11000001))
    (@a . (compile-@!a #b00000011))
    (!a . (compile-@!a #b00100011))
    (dup . (compile-no-argument #b01000011))
    (swap . (compile-no-argument #b01001011))
    (drop . (compile-no-argument #b01010011))
    (rot . (compile-no-argument #b01011011))
    (over . (compile-no-argument #b01100011))
    (reserved . (compile-no-argument #b01101011))
    (add . (compile-no-argument #b01110011))
    (sub . (compile-no-argument #b01111011))
    (inc . (compile-no-argument #b10000011))
    (dec . (compile-no-argument #b10001011))
    (mul . (compile-no-argument #b10010011))
    (xor . (compile-no-argument #b10011011))
    (and . (compile-no-argument #b10100011))
    (or . (compile-no-argument #b10101011))
    (not . (compile-no-argument #b10110011))
    (reserved . (compile-no-argument #b10111011))
    (lsl . (compile-no-argument #b11000011))
    (lsr . (compile-no-argument #b11001011))
    (rol . (compile-no-argument #b11010011))
    (ror . (compile-no-argument #b11011011))
    (cmp . (compile-no-argument #b11100011))
    (reserved . (compile-no-argument #b11101011))
    (stop . (compile-no-argument #b11110011))
    (nop . (compile-no-argument #b11111011))
    (.org . (compile-.org))
    (.db . (compile-.db))
    (.dw . (compile-.dw))))

(defconstant +z-flag+ #b00000001)
(defconstant +c-flag+ #b00000010)
         
(defparameter *pc* 0)
(defparameter *a* 0)
(defparameter *s* 0)
(defparameter *stopped* nil)
(defparameter *data-stack* nil)
(defparameter *return-stack* nil)
(defparameter *memory* nil)
(defparameter *labels* nil)
(defparameter *first-pass* nil)

(defun add-byte (byte)
  (setf (aref *memory* *pc*) byte)
  (incf *pc*))

(defun compile-byte-or-word (token)
  (cond
   ((keywordp token)
    (let ((label-value (gethash token *labels*)))
      (unless label-value
        (unless *first-pass* (error "undefined label ~a" token))
        (setf label-value 0))
      label-value))
   ((numberp token) token)
   (t (error "value expected"))))

(defun compile-push (tokens byte)
  (let ((source (pop tokens)))
    (cond
     ((eql source 'a)
      (when (eql (pop tokens) '+pc)
        (setf byte (logior byte #b00000100)))
      (add-byte byte))
     ((eql source 'pc)
      (setf byte (logior byte #b00010000))
      (when (eql (pop tokens) '+pc)
        (setf byte (logior byte #b00000100)))
      (add-byte byte))
     ((eql source 's)
      (setf byte (logior byte #b00100000))
      (when (eql (pop tokens) '+pc)
        (setf byte (logior byte #b00000100)))
      (add-byte byte))
     ((eql source '\#)
      (setf byte (logior byte #b00110000))
      (let ((imm-value (pop tokens)))
        (cond
         ((eql imm-value 0))
         ((eql imm-value 1)
          (setf byte (logior byte #b01000000)))
         (t (error "0 or 1 expected"))))
      (when (eql (pop tokens) '+pc)
        (setf byte (logior byte #b00000100)))
      (add-byte byte))
     ((eql source 'byte)
      (let ((address (compile-byte-or-word (pop tokens))))
        (when (eql (pop tokens) '+pc)
          (setf address (- *pc* address))
          (when (> address #x7f) (error "address out of range"))
          (when (< address #x-80) (error "address out of range"))
          (setf byte (logior byte #b00000100)))
        (add-byte (logior byte #b10110000))
        (add-byte address)))
     ((eql source 'word)
      (let ((address (compile-byte-or-word (pop tokens))))
        (when (eql (pop tokens) '+pc)
          (setf address (- *pc* address))
          (when (> address #x7fff) (error "address out of range"))
          (when (< address #x-8000) (error "address out of range"))
          (setf byte (logior byte #b00000100)))
        (add-byte (logior byte #b11110000))
        (add-byte address)))
     (t (error "A, PC, S, #, byte or word source expected")))))

(defun compile-pop (tokens byte)
  (let ((destination (pop tokens)))
    (cond
     ((eql destination 'a))
     ((eql destination 'pc)
      (setf byte (logior byte #b00001000)))
     ((eql destination 's)
      (setf byte (logior byte #b00010000)))
     (t (error "A, PC or S destination expected"))))
  (when (eql (pop tokens) '+pc)
    (setf byte (logior byte #b00000100)))
  (add-byte byte))

(defun compile-load (tokens byte)
  (let ((type (pop tokens)))
    (cond
     ((eql type '\#)
      (setf byte (logior byte #b00110000))
      (let ((imm-value (pop tokens)))
        (cond
         ((eql imm-value 0))
         ((eql imm-value 1)
          (setf byte (logior byte #b01000000)))
         (t (error "0 or 1 expected"))))
      (when (eql (pop tokens) '+pc)
        (setf byte (logior byte #b00000100)))
      (add-byte byte))
     ((eql type 'byte)
      (let ((address (compile-byte-or-word (pop tokens))))
        (when (eql (pop tokens) '+pc)
          (setf address (- *pc* address))
          (when (> address #x7f) (error "address out of range"))
          (when (< address #x-80) (error "address out of range"))
          (setf byte (logior byte #b00000100)))
        (add-byte (logior byte #b10110000))
        (add-byte address)))
     ((eql type 'word)
      (let ((address (compile-byte-or-word (pop tokens))))
        (when (eql (pop tokens) '+pc)
          (setf address (- *pc* address))
          (when (> address #x7fff) (error "address out of range"))
          (when (< address #x-8000) (error "address out of range"))
          (setf byte (logior byte #b00000100)))
        (add-byte (logior byte #b11110000))
        (add-byte address)))
     (t (error "#, byte or word expected")))))

(defun compile-branch (tokens byte)
  (let ((token (pop tokens)))
    (when (eql token 'port)
      (setf byte (logior byte #b00010000)))
    (cond
     ((eql token 'a)
      (when (eql (pop tokens) '+pc)
        (setf byte (logior byte #b00000100)))
      (add-byte byte))
     ((eql token 'r)
      (when (eql (pop tokens) '+pc)
        (setf byte (logior byte #b00000100)))
      (add-byte byte)
      (add-byte (logior byte #b00010000)))
     ((eql token 'byte)
      (let ((address (compile-byte-or-word (pop tokens))))
        (when (eql (pop tokens) '+pc)
          (setf address (- address *pc*))
          (when (> address #x7f) (error "address out of range"))
          (when (< address #x-80) (error "address out of range"))
          (when (< address 0) (setf address (- #x7f address)))
          (setf byte (logior byte #b00000100)))
        (add-byte (logior byte #b00011000))
        (add-byte address)))
     ((eql token 'word)
      (let ((address (compile-byte-or-word (pop tokens))))
        (when (eql (pop tokens) '+pc)
          (setf address (- address *pc*))
          (when (> address #x7fff) (error "address out of range"))
          (when (< address #x-8000) (error "address out of range"))
          (when (< address 0) (setf address (- #x7fff address)))
          (setf byte (logior byte #b00000100)))
        (add-byte (logior byte #b00111000))
        (add-byte (ash address -8))
        (add-byte (logand address #xff))))
     (t (error "byte or word transfer width expected")))))

(defun compile-@!a (tokens byte)
  (let ((token (pop tokens)))
    (when (eql token 'port)
      (setf byte (logior byte #b00010000)
            token (pop tokens)))
    (cond
     ((eql token 'byte))
     ((eql token 'word) (setf byte (logior byte #b00001000)))
     (t (error "byte or word transfer width expected")))
    (let ((token (pop tokens)))
      (if token
          (unless (eql token 'popa)
            (error "popa or no argument expected"))
        (setf byte (logior byte #b00000100)))
      (add-byte byte))))

(defun compile-no-argument (tokens byte)
  (let ((token (pop tokens)))
    (if token
        (unless (eql token 'popa)
          (error "popa or no argument expected"))
      (setf byte (logior byte #b00000100)))
    (add-byte byte)))

(defun compile-.org (tokens byte)
  (declare (ignore byte))
  (setf *pc* (compile-byte-or-word (pop tokens))))
  
(defun compile-.db (tokens byte)
  (declare (ignore byte))
  (loop for token in tokens do
        (add-byte (compile-byte-or-word token))))

(defun compile-.dw (tokens byte)
  (declare (ignore byte))
  (loop for token in tokens do
        (let ((word (compile-byte-or-word token)))
          (add-byte (ash word -8))
          (add-byte (logand word #xff)))))

(defun compile-command (element)
  (destructuring-bind (command-symbol . tokens) element
    (let ((command (assoc command-symbol +commands+)))
      (unless command (error "unknown command ~a" command-symbol))
      (let ((command-info (cdr command)))
        (let ((command-function (first command-info))
              (command-byte (second command-info)))
          (funcall (symbol-function command-function) tokens command-byte))))))

(defun compile-element (element)
  (cond
   ((listp element) (compile-command element))
   (t (setf (gethash element *labels*) *pc*))))

(defun forth-pass (program)
  (setf *pc* 0
        *memory* (make-array +max-memory+
                             :element-type '(unsigned-byte 8) 
                             :initial-element 0))
  (loop for element in program do
        (compile-element element)))

(defun forth-asm (program)
  ;; first pass for setting label addresses for labels which are defined later than used
  (setf *labels* (make-hash-table)
        *first-pass* t)
  (forth-pass program)

  ;; second pass with all labels defined
  (setf *first-pass* nil)
  (forth-pass program))

(defun bits-setp (byte mask &optional (bits-byte nil))
  (unless bits-byte (setf bits-byte mask))
  (= (logand byte mask) bits-byte))

(defun get-byte (with-sign)
  (let ((value (aref *memory* *pc*)))
    (incf *pc*)
    (if (and with-sign (> value #x7f))
        (- #x7f value)
      value)))

(defun get-word (with-sign)
  (let ((value (logior (ash (get-byte nil) 8) (get-byte nil))))
    (if (and with-sign (> value #x7fff))
        (- #x7fff value)
      value)))

(defun execute-push (command)
  (let ((current-pc *pc*))
    (incf *pc*)
    (let* ((+pc (bits-setp command #b00000100))
           (destination-return-stack (bits-setp command #b00001000))
           (source
            (cond
             ((bits-setp command #b00110000 #b00000000) *a*)
             ((bits-setp command #b00110000 #b00010000) current-pc)
             ((bits-setp command #b00110000 #b00100000) *s*)
             ((bits-setp command #b00110000)
              (cond
               ((bits-setp command #b11000000 #b00000000) 0)
               ((bits-setp command #b11000000 #b01000000) 1)
               ((bits-setp command #b11000000 #b10000000) (get-byte +pc))
               ((bits-setp command #b11000000 #b11000000) (get-word +pc)))))))
      (when +pc (incf source current-pc))
      (if destination-return-stack
          (push source *return-stack*)
        (push source *data-stack*)))))
        
(defun execute-pop (command)
  (let ((current-pc *pc*))
    (incf *pc*)
    (let* ((+pc (bits-setp command #b00000100))
           (source
            (if (bits-setp command #b00100000)
                (if (bits-setp command #b01000000)
                    (pop *return-stack*)
                  (pop *data-stack*))
              (cond
               ((bits-setp command #b11000000 #b00000000) 0)
               ((bits-setp command #b11000000 #b01000000) 1)
               ((bits-setp command #b11000000 #b10000000) (get-byte +pc))
               ((bits-setp command #b11000000 #b11000000) (get-word +pc))))))
      (when +pc (incf source current-pc))
      (cond
       ((bits-setp command #b00011000 #b00000000) (setf *a* source))
       ((bits-setp command #b00011000 #b00001000) (setf *pc* source))
       ((bits-setp command #b00011000 #b00010000) (setf *s* source))
       ((bits-setp command #b00011000 #b00011000)
        (push *pc* *return-stack*)
        (setf *pc* source))))))

(defun set-flag (flag)
  (setf *s* (logior *s* flag)))

(defun clear-flag (flag)
  (setf *s* (logand *s* (- #xffff flag))))

(defun 1-if-carry ()
  (if (bits-setp *s* +c-flag+)
      1
    0))

(defun execute-branch (command)
  (let ((current-pc *pc*))
    (incf *pc*)
    (let ((+pc (bits-setp command #b00000100)))
      (let ((address
             (cond
              ((bits-setp command #b00011000 #b00000000) *a*)
              ((bits-setp command #b00011000 #b00001000) (pop *data-stack*))
              ((bits-setp command #b00011000 #b00010000) (pop *return-stack*))
              ((bits-setp command #b00011000 #b00011000)
               (if (bits-setp command #b00100000)
                   (get-word +pc)
                 (get-byte +pc))))))
        (when +pc (incf address current-pc))
        (cond
         ((bits-setp command #b11000000 #b00000000)
          (when (bits-setp *s* +c-flag+ 0)
            (setf *pc* address)))
         ((bits-setp command #b11000000 #b01000000)
          (when (bits-setp *s* +c-flag+)
            (setf *pc* address)))
         ((bits-setp command #b11000000 #b10000000)
          (when (bits-setp *s* +z-flag+ 0)
            (setf *pc* address)))
         ((bits-setp command #b11000000 #b11000000)
          (when (bits-setp *s* +z-flag+)
            (setf *pc* address))))))))

(defun execute-other (command)
  (incf *pc*)
  (let ((popa (bits-setp command #b00000100 #b00000000)))
    (cond
     ((bits-setp command #b11100000 #b00000000)  ; @A
      (let ((data
             (if (bits-setp command #b00010000)  ; port
                 0
               (if (bits-setp command #b00001000)  ; 16 bit
                   (logior (ash (aref *memory* *a*) -8)
                           (aref *memory* (1+ *a*)))
                 (aref *memory* *a*)))))
        (push data *data-stack*)))
     ((bits-setp command #b11100000 #b00100000)  ; !A
      (let ((port (bits-setp command #b00010000))
            (data (pop *data-stack*)))
        (unless port
          (if (bits-setp command #b00001000)
            ; 16 bit
              (setf (aref *memory* *a*) (ash data -8)
                    (aref *memory* (1+ *a*)) (logand data #xff))
          ; 8 bit
            (setf (aref *memory* *a*) (logand data #xff))))))
     ((bits-setp command #b11111000 #b01000000)  ; dup
      (push (car *data-stack*) *data-stack*))
     ((bits-setp command #b11111000 #b01001000)  ; swap
      (rotatef (car *data-stack*) (cadr *data-stack*)))
     ((bits-setp command #b11111000 #b01010000)  ; drop
      (pop *data-stack*))
     ((bits-setp command #b11111000 #b01011000)  ; rot
      (rotatef (car *data-stack*) (cadr *data-stack*) (cdadr *data-stack*)))
     ((bits-setp command #b11111000 #b01100000)  ; over
      (push (cadr *data-stack*) *data-stack*))
     ((bits-setp command #b11111000 #b01101000))  ; reserved
     ((bits-setp command #b11111000 #b01110000)  ; add
      (let* ((d (pop *data-stack*))
             (d2 (pop *data-stack*))
             (result (+ d d2 (1-if-carry))))
        (if (> result #xffff)
            (progn
              (decf result #x10000)
              (set-flag +c-flag+))
          (clear-flag +c-flag+))
        (if (zerop result)
            (set-flag +z-flag+)
          (clear-flag +z-flag+))
        (push result *data-stack*)))
     ((bits-setp command #b11111000 #b01111000)  ; sub
      (let* ((d (pop *data-stack*))
             (d2 (pop *data-stack*))
             (result (- d2 d (1-if-carry))))
        (if (< result 0)
            (progn
              (incf result #x10000)
              (set-flag +c-flag+))
          (clear-flag +c-flag+))
        (if (zerop result)
            (set-flag +z-flag+)
          (clear-flag +z-flag+))
        (push result *data-stack*)))
     ((bits-setp command #b11111000 #b10000000)  ; inc
      (let ((d (pop *data-stack*)))
        (incf d)
        (if (> d #xffff)
            (progn
              (setf d 0)
              (set-flag +c-flag+))
          (clear-flag +c-flag+))
        (if (zerop d)
            (set-flag +z-flag+)
          (clear-flag +z-flag+))
        (push d *data-stack*)))
     ((bits-setp command #b11111000 #b10001000)  ; dec
      (let ((d (pop *data-stack*)))
        (decf d)
        (if (< d 0)
            (progn
              (setf d #xffff)
              (set-flag +c-flag+))
          (clear-flag +c-flag+))
        (if (zerop d)
            (set-flag +z-flag+)
          (clear-flag +z-flag+))
        (push d *data-stack*)))
     ((bits-setp command #b11111000 #b10010000)  ; mul
      (let* ((d (pop *data-stack*))
             (d2 (pop *data-stack*))
             (result (* d d2)))
        (if (> result #xffff)
            (progn
              (setf result (logand result #xffff))
              (set-flag +c-flag+))
          (clear-flag +c-flag+))
        (if (zerop result)
            (set-flag +z-flag+)
          (clear-flag +z-flag+))
        (push result *data-stack*)))
     ((bits-setp command #b11111000 #b10011000)  ; xor
      (let* ((d (pop *data-stack*))
             (d2 (pop *data-stack*))
             (result (logxor d d2)))
        (if (zerop result)
            (set-flag +z-flag+)
          (clear-flag +z-flag+))
        (push result *data-stack*)))
     ((bits-setp command #b11111000 #b10100000)  ; and
      (let* ((d (pop *data-stack*))
             (d2 (pop *data-stack*))
             (result (logand d d2)))
        (if (zerop result)
            (set-flag +z-flag+)
          (clear-flag +z-flag+))
        (push result *data-stack*)))
     ((bits-setp command #b11111000 #b10101000)  ; or
      (let* ((d (pop *data-stack*))
             (d2 (pop *data-stack*))
             (result (logior d d2)))
        (if (zerop result)
            (set-flag +z-flag+)
          (clear-flag +z-flag+))
        (push result *data-stack*)))
     ((bits-setp command #b11111000 #b10110000)  ; not
      (let* ((d (pop *data-stack*))
             (result (logxor d #xffff)))
        (if (zerop result)
            (set-flag +z-flag+)
          (clear-flag +z-flag+))
        (push result *data-stack*)))
     ((bits-setp command #b11111000 #b10111000))  ; reserved
     ((bits-setp command #b11111000 #b11000000))  ; lsl
     ((bits-setp command #b11111000 #b11001000))  ; lsr
     ((bits-setp command #b11111000 #b11010000))  ; rol
     ((bits-setp command #b11111000 #b11011000))  ; ror
     ((bits-setp command #b11111000 #b11100000))  ; cmp
     ((bits-setp command #b11111000 #b11101000))  ; reserved
     ((bits-setp command #b11111000 #b11110000)
      (setf *stopped* t))  ; stop
     ((bits-setp command #b11111000 #b11111000)))  ; nop
    (when popa
      (setf *a* (pop *data-stack*)))))

(defun forth-single-step ()
  (let ((command (aref *memory* *pc*)))
    (cond
     ((bits-setp command #b00000011 #b00000000) (execute-push command))
     ((bits-setp command #b00000011 #b00000010) (execute-pop command))
     ((bits-setp command #b00000011 #b00000001) (execute-branch command))
     ((bits-setp command #b00000011 #b00000011) (execute-other command)))))

(defun forth-execute (address)
  (setf *pc* address
        *a* 0
        *s* 0
        *stopped* nil)
  (loop do
        (forth-single-step)
        (when *stopped* (loop-finish))))

(defun test ()
  ; compile test program
  (forth-asm
   '(
     (.org 0)
     (.db #x31 #x32 #x33 #x34 #x35 #x36)
     (.db #x51 #x52 #x53 #x54 #x55 #x56)

     ; swap bytes from 0 to 5 with bytes from 6 to 11
     (.org #x10)
     (push byte 5)
     :loop (dup popa)
     (@A byte)
     (over)
     (push byte 6)
     (add popa)
     (@A byte)
     (swap)
     (!A byte)
     (over popa)
     (!A byte)
     (dec)
     (bcc byte :loop +PC)
     (drop)
     (stop)
     ))
  
  ; execute program
  (forth-execute #x10)

  ; dump memory
  (loop for i from #x0 to #x30 do
        (format t "~2,'0X ~8,'0B~%" (aref *memory* i) (aref *memory* i))))