(in-package #:lempel)

(declaim (optimize (debug 3) (safety 3)))

(declaim (ftype (function ((array (unsigned-byte 8)) (unsigned-byte 8)) (array (unsigned-byte 8)))
                prefix-decode))
(defun prefix-decode (input prefix)
  (let ((bits (make-array (* 8 (length input)) :element-type '(unsigned-byte 1)))
        (decoded-input (queue:new))
        (point-count 0))
    (dotimes (i (length input))
      (byte->bits (elt input i) bits (* 8 i)))
    (do ((i 0)) ((>= i (length bits)) nil)
      (cond ((and (<= i (- (length bits) 8))
                  (= (bits->byte bits i) prefix))
             (incf point-count)
             (let ((point (byte-pair->point (bits->byte bits (+ i 8))
                                            (bits->byte bits (+ i 16)))))
               (if (= (point-size point) 0)
                 (progn
                   (let ((prefix-bits (make-array 8 :element-type '(unsigned-byte 1))))
                     (byte->bits prefix prefix-bits 0)
                     (dotimes (j 8)
                       (queue:push (elt prefix-bits j) decoded-input))
                     (incf i 16)))
                 (progn
                   (setf (point-size point) (* 8 (point-size point)))
                   (setf (point-offset point) (* 8 (point-offset point)))
                   (queue:push point decoded-input)
                   (incf i 24)))))
            (t (let ((step-size 2))
                 (dotimes (j step-size)
                   (queue:push (elt bits (+ i j)) decoded-input))
                 (incf i step-size)))))
    (format t "point count: ~a~&" point-count)
    (let* ((uncompressed-data (decompress (queue:to-list decoded-input)))
           (output (make-array (/ (length uncompressed-data) 8) :element-type '(unsigned-byte 8))))
      (dotimes (i (length output))
        (setf (elt output i) (bits->byte uncompressed-data (* i 8))))
      output)))

(declaim (ftype (function (list) (array (unsigned-byte 1))) decompress))
(defun decompress (data)
  (let ((uncompressed-size
          (let ((size 0))
            (dolist (point data)
              (cond ((point-p point)
                     (incf size (point-size point)))
                    (t (incf size))))
            size))
        (fill-count 0))
    (let ((result (make-array uncompressed-size :element-type '(unsigned-byte 1))))
      (format t "uncompressed size: ~a bits ~a bytes~&" (length result) (/ (length result) 8))
      (dolist (point data)
        (cond ((point-p point)
               (let ((start (- fill-count (point-offset point))))
                 (dotimes (j (point-size point))
                   (setf (elt result (+ fill-count j)) (if (> (+ start j) 0)
                                                         (elt result (+ start j))
                                                         0)))
                 (incf fill-count (point-size point))))
              ((typep point '(unsigned-byte 1))
               (setf (elt result fill-count) point)
               (incf fill-count))
              (t (error (format nil "cannot handle type ~a" (type-of point))))))
      (assert (= fill-count (length result)))
      result)))

(declaim (ftype (function ((unsigned-byte 8) (unsigned-byte 8)) point) byte-pair->point))
(defun byte-pair->point (b1 b2)
  (let ((offset (if reversed-byte-order b2 b1))
        (size (if reversed-byte-order b1 b2))
        (max-word-length (1- (ash 1 (- 16 window-bits))))
        (max-offset-length (1- (ash 1 window-bits))))
    (if (>= window-bits 8)
      (make-point :offset (logior offset
                                  (ash (logand size (logxor #xff max-word-length))
                                       (- window-bits 8)))
                  :size (logand size max-word-length))
      (make-point :offset (logand offset max-offset-length)
                  :size (logior size (ash (logand offset (logxor #xff max-offset-length))
                                          (- 16 window-bits 8)))))))