(defvar *fork-depth* 4)
(defvar *minimum-par-length* 8192)
(deftype array-index ()
  `(and fixnum unsigned-byte))

(defmacro par (length &rest forms)
  (let ((depth-gensym (gensym "FORK-DEPTH")))
    `(if (or (zerop *fork-depth*)
             (< ,length *minimum-par-length*))
         (progn ,@forms)
         (let* ((,depth-gensym *fork-depth*)
                (threads (list
                          ,@(loop for form in forms
                                  collect `(bt:make-thread
                                            (lambda ()
                                              (let ((*fork-depth* (1- ,depth-gensym)))
                                                ,form)))))))
           (mapc #'bt:join-thread threads)))))

(declaim (ftype (function * (simple-array (unsigned-byte 32) 1)) merge*))
(defun merge* (vector start midpoint end result)
  "Merge (subseq vector start midpoint) and (subseq vector midpoint end) into (subseq result start end)."
  (declare ((simple-array (unsigned-byte 32) 1) vector result)
           (optimize (speed 3) (safety 0))
           (array-index start midpoint end))
  (let* ((length (- end start))
         (output-position 0)
         (position-1 start)
         (position-2 midpoint))
    (declare (array-index output-position position-1 position-2))
    (loop
      (when (= output-position length)
        (return))
      (when (= position-1 midpoint)
        (replace result vector
                 :start1 (+ output-position start)
                 :start2 position-2
                 :end1 end)
        (return))
      (when (= position-2 end)
        (replace result vector
                 :start1 (+ output-position start)
                 :start2 position-1
                 :end1 end)
        (return))
      (let ((value-1 (aref vector position-1))
            (value-2 (aref vector position-2)))
        (cond
          ((< value-2 value-1)
           (setf (aref result (+ start output-position)) (aref vector position-2))
           (incf output-position)
           (incf position-2))
          (t
           (setf (aref result (+ start output-position)) (aref vector position-1))
           (incf output-position)
           (incf position-1)))))
    result))

(defun %merge-sort! (vector start end temporary-vector)
  "Merge-sort (subseq vector start end), using temporary-vector as scratch space."
  (declare ((simple-array (unsigned-byte 32) 1) vector temporary-vector)
           (array-index start end)
           (optimize (speed 3) (safety 0)))
  (let ((length (- end start)))
    (cond
      ((< length 2) #| This must be sorted already |#)
      ((= length 2)
       (let ((second (aref vector (1- end)))
             (first  (aref vector start)))
         (when (< second first)
           (setf (aref vector start)    second
                 (aref vector (1- end)) first))))
      (t
       (let ((midpoint (+ start (floor length 2))))
         (declare (array-index midpoint))
         (par length
              (%merge-sort! vector start midpoint temporary-vector)
              (%merge-sort! vector midpoint end temporary-vector))
         (merge* vector start midpoint end temporary-vector)
         (replace vector temporary-vector
                  :start1 start :end1 end
                  :start2 start :end2 end))))))

(defun merge-sort! (vector)
  (%merge-sort! vector 0
                (length vector)
                (make-array (length vector)
                            :element-type '(unsigned-byte 32)))
  vector)