ocm/s2.lisp

(in-package :cl-user)
(defpackage #:ocm.s2
  (:use :cl))
(in-package :ocm.s2)

;; In the process of converting a latitude-longitude pair to a 64-bit cell
;; id, the following coordinate systems are used:
;;
;;  (id)
;;    An S2CellId is a 64-bit encoding of a face and a Hilbert curve position
;;    on that face.  The Hilbert curve position implicitly encodes both the
;;    position of a cell and its subdivision level (see s2cell_id.h).
;;
;;  (face, i, j)
;;    Leaf-cell coordinates.  "i" and "j" are integers in the range
;;    [0,(2**30)-1] that identify a particular leaf cell on the given face.
;;    The (i, j) coordinate system is right-handed on each face, and the
;;    faces are oriented such that Hilbert curves connect continuously from
;;    one face to the next.
;;
;;  (face, s, t)
;;    Cell-space coordinates.  "s" and "t" are real numbers in the range
;;    [0,1] that identify a point on the given face.  For example, the point
;;    (s, t) = (0.5, 0.5) corresponds to the center of the top-level face
;;    cell.  This point is also a vertex of exactly four cells at each
;;    subdivision level greater than zero.
;;
;;  (face, si, ti)
;;    Discrete cell-space coordinates.  These are obtained by multiplying
;;    "s" and "t" by 2**31 and rounding to the nearest unsigned integer.
;;    Discrete coordinates lie in the range [0,2**31].  This coordinate
;;    system can represent the edge and center positions of all cells with
;;    no loss of precision (including non-leaf cells).  In binary, each
;;    coordinate of a level-k cell center ends with a 1 followed by
;;    (30 - k) 0s.  The coordinates of its edges end with (at least)
;;    (31 - k) 0s.
;;
;;  (face, u, v)
;;    Cube-space coordinates in the range [-1,1].  To make the cells at each
;;    level more uniform in size after they are projected onto the sphere,
;;    we apply a nonlinear transformation of the form u=f(s), v=f(t).
;;    The (u, v) coordinates after this transformation give the actual
;;    coordinates on the cube face (modulo some 90 degree rotations) before
;;    it is projected onto the unit sphere.
;;
;;  (face, u, v, w)
;;    Per-face coordinate frame.  This is an extension of the (face, u, v)
;;    cube-space coordinates that adds a third axis "w" in the direction of
;;    the face normal.  It is always a right-handed 3D coordinate system.
;;    Cube-space coordinates can be converted to this frame by setting w=1,
;;    while (u,v,w) coordinates can be projected onto the cube face by
;;    dividing by w, i.e. (face, u/w, v/w).
;;
;;  (x, y, z)
;;    Direction vector (S2Point).  Direction vectors are not necessarily unit
;;    length, and are often chosen to be points on the biunit cube
;;    [-1,+1]x[-1,+1]x[-1,+1].  They can be be normalized to obtain the
;;    corresponding point on the unit sphere.
;;
;;  (lat, lng)
;;    Latitude and longitude (S2LatLng).  Latitudes must be between -90 and
;;    90 degrees inclusive, and longitudes must be between -180 and 180
;;    degrees inclusive.
;;
;; Note that the (i, j), (s, t), (si, ti), and (u, v) coordinate systems are
;; right-handed on all six faces.

(defparameter +swap-mask+ 1)
(defparameter +invert-mask+ 2)
(defparameter +max-cell-level+ 30 "This is the number of levels needed to specify a leaf cell.")
(defparameter +limit-ij+ (ash 1 +max-cell-level+) "The maximum index of a valid leaf cell plus one. The range of valid leaf cell indices is [0..+limit-ij+-1].")
(defparameter +max-si-ti+ (ash 1 (+ 1 +max-cell-level+)) "The maximum value of an si- or ti-coordinate.  The range of valid (si,ti) values is [0..+max-si-ti+].")

;; An S2CellId is a 64-bit unsigned integer that uniquely identifies a
;; cell in the S2 cell decomposition.  It has the following format:
;;
;;   id = [face][face_pos]
;;
;;   face:     a 3-bit number (range 0..5) encoding the cube face.
;;
;;   face_pos: a 61-bit number encoding the position of the center of this
;;             cell along the Hilbert curve over this face (see the Wiki
;;             pages for details).
;;
;; Sequentially increasing cell ids follow a continuous space-filling curve
;; over the entire sphere.  They have the following properties:
;;
;;  - The id of a cell at level k consists of a 3-bit face number followed
;;    by k bit pairs that recursively select one of the four children of
;;    each cell.  The next bit is always 1, and all other bits are 0.
;;    Therefore, the level of a cell is determined by the position of its
;;    lowest-numbered bit that is turned on (for a cell at level k, this
;;    position is 2 * (kMaxLevel - k).)
;;
;;  - The id of a parent cell is at the midpoint of the range of ids spanned
;;    by its children (or by its descendants at any level).
(defparameter +face-bits+ 3)
(defparameter +num-faces+ 6)
(defparameter +max-level+ +max-cell-level+)
(defparameter +pos-bits+ (1+ (* 2 +max-level+)))
(defparameter +max-size+ (ash 1 +max-level+))

(defun st-to-ij (s)
  (max 0 (min (- +limit-ij+ 1) (round (- (* s +limit-ij+) 0.5d0)))))

;; Use quadratic (s,t) -> (u,v) projection. See S2 sources for explanation.
(defun st-to-uv (s)
  (* (/ 1 3)
     (if (>= s 0.5d0)
         (- (* 4 s s) 1)
         (- 1 (* 4 (- s 1) (- s 1))))))
(defun uv-to-st (u)
  (if (>= u 0)
      (* 0.5d0 (sqrt (+ 1 (* 3 u))))
      (- 1 (* 0.5d0 (sqrt (- 1 (* 3 u)))))))

(defstruct point
  (x 0.0d0 :type double-float)
  (y 0.0d0 :type double-float)
  (z 0.0d0 :type double-float))

(defstruct latlng
  (lat 0d0 :type double-float)
  (lng 0d0 :type double-float))

(defun deg-to-rad (deg)
  (* (/ pi 180d0) deg))
(defun rad-to-deg (rad)
  (* (/ 180d0 pi) rad))

(defun from-radians (lat-rad lng-rad)
  (make-latlng :lat lat-rad :lng lng-rad))
(defun from-degrees (lat-deg lng-deg)
  (make-latlng :lat (deg-to-rad lat-deg) :lng (deg-to-rad lng-deg)))
(defun to-degrees (latlng)
  (declare (type latlng latlng))
  (with-slots (lat lng) latlng
    (cons (rad-to-deg lat) (rad-to-deg lng))))
(defun latlng-to-point (latlng)
  (declare (type latlng latlng))
  (with-slots (lat lng) latlng
    (let* ((phi lat)
           (theta lng)
           (cos-phi (cos phi)))
      (make-point :x (* (cos theta) cos-phi)
                  :y (* (sin theta) cos-phi)
                  :z (sin phi)))))
(defun point-lat (p)
  (declare (type point p))
  (with-slots (x y z) p
    (atan z (sqrt (+ (* x x) (* y y))))))
(defun point-lng (p)
  (declare (type point p))
  (with-slots (x y z) p
    (atan y x)))
(defun point-to-lat-lng (p)
  (declare (type point p))
  (from-radians (point-lat p) (point-lng p)))

(defun dot (p1 p2)
  (declare (type point p1 p2))
  (+ (* (point-x p1) (point-x p2))
     (* (point-y p1) (point-y p2))
     (* (point-z p1) (point-z p2))))

(defparameter +face-uvw-axes+
  (make-array '(6 3) :element-type 'point :initial-contents
              (list
               (list (make-point :y 1d0) (make-point :z 1d0) (make-point :x 1d0))
               (list (make-point :x -1d0) (make-point :z 1d0) (make-point :y 1d0))
               (list (make-point :x -1d0) (make-point :y -1d0) (make-point :z 1d0))
               (list (make-point :z -1d0) (make-point :y -1d0) (make-point :x -1d0))
               (list (make-point :z -1d0) (make-point :x 1d0) (make-point :y -1d0))
               (list (make-point :y 1d0) (make-point :x 1d0) (make-point :z -1d0))))
  "The U,V,W axes for each face.")

(defun get-xyz-face (p)
  (declare (type point p))
  (with-slots (x y z) p
    (cond
      ((and (> (abs x) (abs y))
            (> (abs x) (abs z)))
       (if (>= x 0) 0 3))
      ((and (> (abs y) (abs x))
            (> (abs y) (abs z)))
       (if (>= y 0) 1 4))
      (t
       (if (>= z 0) 2 5)))))

(defun get-uvw-axis (face axis)
  (aref +face-uvw-axes+ face axis))

(defun get-norm (face)
  (get-uvw-axis face 2))

(defun valid-face-xyz-to-uv (face p)
  (declare (type point p))
  (assert (> (dot (get-norm face) p) 0))
  (with-slots (x y z) p
    (ecase face
      (0 (values (/ y x) (/ z x)))
      (1 (values (/ (- x) y) (/ z y)))
      (2 (values (/ (- x) z) (/ (- y) z)))
      (3 (values (/ z x) (/ y x)))
      (4 (values (/ z y) (/ (- x) y)))
      (5 (values (/ (- y) z) (/ (- x) z))))))

(defun xyz-to-face-uv (p)
  (declare (type point p))
  (let ((face (get-xyz-face p)))
    (multiple-value-bind (u v)
        (valid-face-xyz-to-uv face p)
      (values face u v))))

(defparameter +lookup-bits+ 4)
(defvar *lookup-pos* (make-array (ash 1 (+ 2 (* +lookup-bits+ 2))) :element-type '(unsigned-byte 16)))
(defvar *lookup-ij* (make-array (ash 1 (+ 2 (* +lookup-bits+ 2))) :element-type '(unsigned-byte 16)))
(defparameter +ij-to-pos+
  #(#(0 1 3 2) ; canonical order
    #(0 3 1 2) ; axes swapped
    #(2 3 1 0) ; bits inverted
    #(2 1 3 0)); swapped & inverted
  "IJtoPos[orientation][ij] -> pos")
(defparameter +pos-to-ij+
  #(#(0 1 3 2) ; canonical order:    (0,0), (0,1), (1,1), (1,0)
    #(0 2 3 1) ; axes swapped:       (0,0), (1,0), (1,1), (0,1)
    #(3 2 0 1) ; bits inverted:      (1,1), (1,0), (0,0), (0,1)
    #(3 1 0 2)); swapped & inverted: (1,1), (0,1), (0,0), (1,0)
  "PosToIJ[orientation][pos] -> ij")
(defparameter +pos-to-orient+
  (make-array 4 :element-type '(unsigned-byte 8)
              :initial-contents (list +swap-mask+ 0 0 (logior +swap-mask+ +invert-mask+)))
  "PosToOrientation[pos] -> orientation_modifier")

(defun init-lookup ()
  (labels ((init-lookup-cell (level i j orig-orient pos orient)
             (if (= level +lookup-bits+)
                 (let ((ij (+ (ash i +lookup-bits+) j)))
                   (setf (aref *lookup-pos* (+ (ash ij 2) orig-orient)) (+ (ash pos 2) orient))
                   (setf (aref *lookup-ij* (+ (ash pos 2) orig-orient)) (+ (ash ij 2) orient)))
                 (let ((level (1+ level))
                       (i (ash i 1))
                       (j (ash j 1))
                       (pos (ash pos 2))
                       (r (aref +pos-to-ij+ orient)))
                   (init-lookup-cell level (+ i (ash (aref r 0) -1)) (+ j (logand (aref r 0) 1))
                                     orig-orient pos (logxor orient (aref +pos-to-orient+ 0)))
                   (init-lookup-cell level (+ i (ash (aref r 1) -1)) (+ j (logand (aref r 1) 1))
                                     orig-orient (+ pos 1) (logxor orient (aref +pos-to-orient+ 1)))
                   (init-lookup-cell level (+ i (ash (aref r 2) -1)) (+ j (logand (aref r 2) 1))
                                     orig-orient (+ pos 2) (logxor orient (aref +pos-to-orient+ 2)))
                   (init-lookup-cell level (+ i (ash (aref r 3) -1)) (+ j (logand (aref r 3) 1))
                                     orig-orient (+ pos 3) (logxor orient (aref +pos-to-orient+ 3)))))))
    (init-lookup-cell 0 0 0 0 0 0)
    (init-lookup-cell 0 0 0 +swap-mask+ 0 +swap-mask+)
    (init-lookup-cell 0 0 0 +invert-mask+ 0 +invert-mask+)
    (init-lookup-cell 0 0 0 (logior +swap-mask+ +invert-mask+) 0 (logior +swap-mask+ +invert-mask+))))

(eval-when (:execute :compile-toplevel :load-toplevel)
  (init-lookup))

(defun from-face-ij (face i j)
  (let ((n (ash face (1- +pos-bits+)))
        (bits (logand face +swap-mask+)))
    (macrolet ((get-bits (k)
                 `(let ((mask (1- (ash 1 +lookup-bits+))))
                    (setf bits
                          (aref *lookup-pos*
                                (+ bits
                                   (ash (logand mask (ash i (- (* ,k +lookup-bits+)))) (+ +lookup-bits+ 2))
                                   (ash (logand mask (ash j (- (* ,k +lookup-bits+)))) 2))))
                    (setf n (logior n (ash (ash bits -2)
                                           (* ,k 2 +lookup-bits+)))
                          bits (logand bits (logior +swap-mask+ +invert-mask+))))))
      (get-bits 7)
      (get-bits 6)
      (get-bits 5)
      (get-bits 4)
      (get-bits 3)
      (get-bits 2)
      (get-bits 1)
      (get-bits 0))
    (1+ (* 2 n))))

(defun point-to-cell (p)
  (multiple-value-bind (face u v)
      (xyz-to-face-uv p)
    (from-face-ij face
                  (st-to-ij (uv-to-st u))
                  (st-to-ij (uv-to-st v)))))

(defun cell-to-face-ij-orientation (id)
  (let* ((i 0)
         (j 0)
         (face (cell-face id))
         (bits (logand face +swap-mask+)))
    (macrolet ((get-bits (k)
                 `(let ((nbits (if (= ,k 7) (- +max-level+ (* 7 +lookup-bits+)) +lookup-bits+)))
                    (setf bits (aref *lookup-ij*
                                     (+ bits
                                        (ash (logand (ash id (- (1+ (* ,k 2 +lookup-bits+))))
                                                     (1- (ash 1 (* 2 nbits)))) 2))))
                    (setf i (+ i (ash (ash bits (- (+ +lookup-bits+ 2)))
                                      (* ,k +lookup-bits+)))
                          j (+ j (ash (logand (ash bits -2)
                                              (1- (ash 1 +lookup-bits+)))
                                      (* ,k +lookup-bits+)))
                          bits (logand bits (logior +swap-mask+ +invert-mask+))))))
      (get-bits 7)
      (get-bits 6)
      (get-bits 5)
      (get-bits 4)
      (get-bits 3)
      (get-bits 2)
      (get-bits 1)
      (get-bits 0)
      (values face i j (logxor bits (if (zerop (logand (cell-lsb id) #x1111111111111110)) 0 +swap-mask+))))))

(defun cell-get-center-face-si-ti (id)
  (multiple-value-bind (face i j orient) (cell-to-face-ij-orientation id)
    (declare (ignore orient))
    (let ((delta (if (cell-is-leaf id) 1 (if (zerop (logand 1 (logxor i (ash id -2)))) 0 2))))
      (values face (+ delta (* 2 i)) (+ delta (* 2 j))))))

(defun face-si-ti-to-xyz (face si ti)
  (face-uv-ti-xyz face
                  (st-to-uv (si-ti-to-st si))
                  (st-to-uv (si-ti-to-st ti))))

(defun face-uv-ti-xyz (face u v)
  (declare (type double-float u v))
  (ecase face
    (0 (make-point :x 1d0 :y u :z v))
    (1 (make-point :x (- u) :y 1d0 :z v))
    (2 (make-point :x (- u) :y (- v) :z 1d0))
    (3 (make-point :x -1d0 :y (- v) :z (- u)))
    (4 (make-point :x v :y -1d0 :z (- u)))
    (5 (make-point :x v :y u :z -1d0))))

(defun si-ti-to-st (si)
  (assert (<= si +max-si-ti+))
  (* si (/ 1d0 +max-si-ti+)))

(defun cell-to-point-raw (id)
  (apply #'face-si-ti-to-xyz (multiple-value-list (cell-get-center-face-si-ti id))))

(defun find-lsb-set (num)
  (loop for i upto 63 when (logbitp i num) return i))

(defun find-msb-set (num)
  (loop for i from 63 downto 0 when (logbitp i num) return i))

(defun lsb-for-level (level)
  (ash 1 (* 2 (- +max-level+ level))))

(defun cell-face (id)
  (ash id (- +pos-bits+)))

(defun cell-lsb (id)
  (logand id (1+ (lognot id))))

(defun cell-level (id)
  (assert (not (zerop id)))
  (- +max-level+ (ash (find-lsb-set id) -1)))

(defun cell-pos (id)
  (logand id (lognot 0)))

(defun cell-is-valid (id)
  (and (< (cell-face id) +num-faces+)
       (not (zerop (logand (cell-lsb id) #x1555555555555555)))))

(defun cell-child-position (id &optional level)
  (unless level (setf level (cell-level id)))
  (assert (cell-is-valid id))
  (assert (>= level 1))
  (assert (<= level (cell-level id)))
  (logand 3 (ash id (- (1+ (* 2 (- +max-level+ level)))))))

(defun cell-range-min (id)
  (- id (1- (cell-lsb id)) ))
(defun cell-range-max (id)
  (+ id (1- (cell-lsb id))))
(defun cell-is-leaf (id)
  (not (zerop (logand id 1))))
(defun cell-is-face (id)
  (zerop (logand id (1- (lsb-for-level 0)))))

(defun cell-parent (id &optional level)
  (assert (cell-is-valid id))
  (if level (progn
              (assert (>= level 0))
              (assert (<= level (cell-level id))))
      (progn (assert (not (cell-is-face id)))))
  (let ((new-lsb (if level (lsb-for-level level) (ash (cell-lsb id) 2))))
    (logior new-lsb (logand id (1+ (lognot new-lsb))))))

(defun cell-child (id pos)
  (assert (cell-is-valid id))
  (assert (not (cell-is-leaf id)))
  (+ id (* (+ (* 2 pos) 1 -4) (ash (cell-lsb id) -2))))

(defun cell-to-string (id)
  (format nil "~a/~{~a~}" (cell-face id)
          (loop for level from 1 to (cell-level id)
             collect (cell-child-position id level))))

;;;;;;;;;
;;;
(defun make-span (vector offset &optional size)
  (declare (type vector vector))
  (let ((length (length vector)))
    (assert (< offset length))
    (when size (assert (< (+ offset size) length)))
    (make-array (if size size (- length offset))
                :element-type (or (cadr (type-of vector)) t)
                :displaced-to vector
                :displaced-index-offset offset)))

(deftype byte-vector () `(vector (unsigned-byte 8)))

(defun make-encoder (size)
  (make-array size :element-type '(unsigned-byte 8) :fill-pointer 0 :adjustable t))

(defun encoder-avail (encoder)
  (declare (type byte-vector encoder))
  (- (array-total-size encoder)
     (fill-pointer encoder)))

(defun encoder-ensure (encoder size)
  (declare (type byte-vector encoder))
  (when (< (encoder-avail encoder) size)
    (adjust-array encoder (+ (fill-pointer encoder) size))))

(defun encoder-put8 (encoder value)
  (declare (type byte-vector encoder)
           (type (unsigned-byte 8) value))
  (assert (> (encoder-avail encoder) 1))
  (vector-push value encoder)
  encoder)

(defun encoder-putn (encoder vector)
  (declare (type byte-vector encoder)
           (type byte-vector vector))
  (loop for value across vector do (vector-push value encoder))
  encoder)

(defun encoder-length (encoder)
  (declare (type byte-vector encoder))
  (fill-pointer encoder))

(defparameter +varint-max32+ 5 "Maximum varint encoding length for uint32")
(defun encoder-put-varint32 (encoder v)
  (declare (type byte-vector encoder)
           (type (unsigned-byte 32) v))
  (let ((b 128))
    (cond
      ((< v (ash 1 7)) (encoder-put8 encoder v))
      ((< v (ash 1 14))
       (encoder-put8 encoder (logand 255 (logior v b)))
       (encoder-put8 encoder (logand 255 (ash v -7))))
      ((< v (ash 1 21))
       (encoder-put8 encoder (logand 255 (logior v b)))
       (encoder-put8 encoder (logand 255 (logior (ash v -7) b)))
       (encoder-put8 encoder (logand 255 (ash v -14))))
      ((< v (ash 1 28))
       (encoder-put8 encoder (logand 255 (logior v b)))
       (encoder-put8 encoder (logand 255 (logior (ash v -7) b)))
       (encoder-put8 encoder (logand 255 (logior (ash v -14) b)))
       (encoder-put8 encoder (logand 255 (ash v -21))))
      (t
       (encoder-put8 encoder (logand 255 (logior v b)))
       (encoder-put8 encoder (logand 255 (logior (ash v -7) b)))
       (encoder-put8 encoder (logand 255 (logior (ash v -14) b)))
       (encoder-put8 encoder (logand 255 (logior (ash v -21) b)))
       (encoder-put8 encoder (logand 255 (ash v -28))))))
  encoder)

(defparameter +varint-max64+ 10 "Maximum varint encoding length for uint64")
(defun encoder-put-varint64 (encoder v)
  (declare (type byte-vector encoder)
           (type (unsigned-byte 64) v))
  (if (< v (ash 1 28))
      (encoder-put-varint32 encoder v)
      (let ((x32 (logand (1- (ash 1 32)) (logior v (ash 1 7) (ash 1 21))))
            (y32 (logand (1- (ash 1 32)) (logior v (ash 1 14) (ash 1 28)))))
        (encoder-put8 encoder (logand 255 x32))
        (encoder-put8 encoder (logand 255 (ash y32 -7)))
        (encoder-put8 encoder (logand 255 (ash x32 -14)))
        (encoder-put8 encoder (logand 255 (ash y32 -21)))
        (if (< v (ash 1 35))
            (encoder-put8 encoder (logand 255 (ash v -28)))
            (progn
              (encoder-put8 encoder (logand 255 (logior (ash v -28) (ash 1 7))))
              (encoder-put-varint32 encoder (ash v -35)))))))

;;; decoder ;;;
(defun make-decoder (size &key (fill-pointer 0) initial-contents initial-element displaced-to)
  (apply #'make-array size
         :element-type '(unsigned-byte 8)
         :fill-pointer fill-pointer
         (append
          (when initial-contents `(:initial-contents ,initial-contents))
          (when initial-element `(:initial-element ,initial-element))
          (when displaced-to `(:displaced-to ,displaced-to :displaced-index-offset ,(fill-pointer displaced-to))))))

(defun decoder-avail (decoder)
  (declare (type byte-vector decoder))
  (- (array-total-size decoder)
     (fill-pointer decoder)))

(defun decoder-skip (decoder &optional (count 1))
  (declare (type byte-vector decoder)
           (type (unsigned-byte 32) count))
  (incf (fill-pointer decoder) count))

(defun decoder-reset (decoder)
  (declare (type byte-vector decoder))
  (setf (fill-pointer decoder) 0)
  decoder)

(defun make-sub-decoder (decoder size &optional will-decode)
  (when (>= (decoder-avail decoder) size)
    (prog1 (make-decoder size :fill-pointer (if will-decode 0 size) :displaced-to decoder)
      (decoder-skip decoder size))))

(defmacro with-decoder-reset ((decoder &optional (offset 0)) &body body)
  (let ((fp (gensym "FP"))
        (dc (gensym "DC")))
    `(let* ((,dc ,decoder)
            (,fp (fill-pointer ,dc)))
       (unwind-protect
            (progn
              (setf (fill-pointer ,dc) ,offset)
              ,@body)
         (setf (fill-pointer ,dc) ,fp)))))

(defun decoder-get8 (decoder)
  (declare (type byte-vector decoder))
  (prog1 (aref decoder (fill-pointer decoder))
    (decoder-skip decoder)))

(defun decoder-peek (decoder &optional (offset 0))
  (declare (type byte-vector decoder)
           (type (unsigned-byte 32) offset))
  (aref decoder (+ (fill-pointer decoder) offset)))

(defun decoder-get-varint64 (decoder)
  (declare (type byte-vector decoder))
  (loop
     for b = (decoder-get8 decoder) then (decoder-get8 decoder)
     for i from 0
     with result = 0
     do (incf result (ash (if (zerop i) b (1- b)) (* i 7)))
     until (< b 128)
     finally (return result)))

;;;
;;; encoded-uint-vector ;;;
;;;
(defun encode-uint-with-length (value length encoder)
  (declare (type integer value)
           (type (integer 0 8) length)
           (type byte-vector encoder))
  (assert (>= (encoder-avail encoder) length))
  (loop repeat length
     do (encoder-put8 encoder (logand value 255))
     do (setf value (ash value -8)))
  (assert (zerop value))
  (values))

(defun get-uint-with-length (decoder length)
  (declare (type byte-vector decoder)
           (type (integer 0 8) length))
  (let ((value 0))
    (loop for i from (1- length) downto 0
       do (setf value (+ (ash value 8)
                         (decoder-peek decoder i))))
    (decoder-skip decoder length)
    value))

(defun get-type-byte-size (v-type)
  (values
   (typecase v-type
     (list (case (car v-type)
             ((array simple-array vector) (get-type-byte-size (cadr v-type)))
             ((unsigned-byte signed-byte) (ceiling (cadr v-type) 8))
             (integer (when (cadr v-type) (ceiling (log (cadr v-type) 2) 8)))
             (t nil)))
     (symbol (when (eql v-type 'fixnum) 8))
     (t nil))))

(defun encode-uint-vector (v encoder)
  (declare (type vector v)
           (type byte-vector encoder))
  (let ((length (1+ (ash (find-msb-set
                          (loop
                             for value across v
                             with bits = 1
                             do (setf bits (logior bits value))
                             finally (return bits)))
                         -3))))
    (assert (<= 1 length 8))
    (encoder-ensure encoder (+ (* (length v) length)
                               +varint-max64+))
    (encoder-put-varint64 encoder
                          (logior (* (length v) (get-type-byte-size (type-of v)))
                                  (1- length)))
    (loop for value across v
       do (encode-uint-with-length value length encoder))
    encoder))

(defstruct encoded-uint-vector
  (size 0 :type (unsigned-byte 32))
  (length 1 :type (integer 1 8))
  (bytes 8 :type (member 1 2 4 8))
  (data nil :type byte-vector))

(defun encoded-uint-vector-init (decoder &optional (bytes 8))
  (declare (type byte-vector decoder)
           (type (member 1 2 4 8) bytes))
  (let ((size-len (decoder-get-varint64 decoder)))
    (when size-len
      (let* ((size (floor size-len bytes))
             (length (1+ (logand size-len (1- bytes))))
             (bytes-length (* size length))
             (data (make-sub-decoder decoder bytes-length)))
        (when data
          (make-encoded-uint-vector :size size :length length :bytes bytes
                                    :data data))))))

(defun encoded-uint-vector-get (euv pos)
  (declare (type encoded-uint-vector euv)
           (type (unsigned-byte 32) pos))
  (with-slots (size data length) euv
    (assert (< pos size))
    (with-decoder-reset (data (* pos length))
      (get-uint-with-length data length))))

(defun encoded-uint-vector-decode (euv)
  (declare (type encoded-uint-vector euv))
  (let* ((size (encoded-uint-vector-size euv))
         (result (make-array size :element-type '(unsigned-byte 64))))
    (dotimes (i size result)
      (setf (aref result i) (encoded-uint-vector-get euv i)))))

;; encoded-string-vector
(defstruct string-vector-encoder
  (offsets (make-array 0 :element-type '(unsigned-byte 64) :adjustable t :fill-pointer 0)
           :type (vector (unsigned-byte 64)))
  (data (make-encoder 0) :type byte-vector))

(defun string-vector-encoder-add-via-encoder (sve)
  (declare (type string-vector-encoder sve))
  (with-slots (offsets data) sve
    (vector-push-extend (length data) offsets)
    data))

(defun string-vector-encoder-size (sve)
  (declare (type string-vector-encoder sve))
  (length (string-vector-encoder-offsets sve)))

(defun string-vector-encoder-encode (sve encoder)
  (declare (type string-vector-encoder sve)
           (type byte-vector encoder))
  (with-slots (offsets data) sve
    (vector-push-extend (length data) offsets)
    (encode-uint-vector (make-span offsets 1) encoder)
    (encoder-ensure encoder (encoder-length data))
    (encoder-putn encoder data)
    encoder))

(defstruct encoded-string-vector
  (offsets nil :type encoded-uint-vector)
  (data nil :type byte-vector))

(defun encoded-string-vector-init (decoder)
  (declare (type byte-vector decoder))
  (let ((offsets (encoded-uint-vector-init decoder 8)))
    (when offsets
      (let* ((size (encoded-uint-vector-size offsets))
             (length (if (zerop size) 0 (encoded-uint-vector-get offsets (1- size))))
             (data (make-sub-decoder decoder length)))
        (when data
          (make-encoded-string-vector :offsets offsets :data data))))))

(defun encoded-string-vector-size (esv)
  (declare (type encoded-string-vector esv))
  (encoded-uint-vector-size (encoded-string-vector-offsets esv)))

(defun encoded-string-vector-get (esv index &optional fill-pointer)
  (declare (type encoded-string-vector esv)
           (type (unsigned-byte 32) index))
  (with-slots (offsets data) esv
      (let* ((start (if (zerop index) 0 (encoded-uint-vector-get offsets (1- index))))
             (length (- (encoded-uint-vector-get offsets index) start)))
        (with-decoder-reset (data start)
          (make-decoder length :displaced-to data :fill-pointer fill-pointer)))))

(defun encoded-string-vector-decode (esv)
  (declare (type encoded-string-vector esv))
  (let* ((size (encoded-string-vector-size esv))
         (result (make-array size :element-type '(vector (unsigned-byte 8)))))
    (dotimes (i size result)
      (setf (aref result i) (encoded-string-vector-get esv i)))))

;; encoded-cell-id-vector
(defstruct encoded-cell-id-vector
  (deltas nil :type encoded-uint-vector)
  (base 0 :type (unsigned-byte 64))
  (shift 0 :type (unsigned-byte 8)))

(defun encoded-cell-id-vector-size (ecv)
  (declare (type encoded-cell-id-vector ecv))
  (with-slots (deltas) ecv
    (when deltas
      (encoded-uint-vector-size deltas))))

(defun encoded-cell-id-vector-get (ecv i)
  (declare (type encoded-cell-id-vector ecv)
           (type (unsigned-byte 32) i))
  (with-slots (deltas base shift) ecv
    (+ base
       (ash (encoded-uint-vector-get deltas i) shift))))

(defun encoded-cell-id-vector-init (decoder)
  (declare (type byte-vector decoder))
  (when (>= (decoder-avail decoder) 2)
    (let* ((code+len (decoder-get8 decoder))
           (shift-code (ash code+len -3)))
      (when (= shift-code 31)
        (setf shift-code (+ 29 (decoder-get8 decoder))))
      (let* ((base-len (logand code+len 7))
             (base (get-uint-with-length decoder base-len))
             shift)
        (when base
          (setf base (ash base (- 64 (* 8 (max 1 base-len)))))
          (if (>= shift-code 29)
              (setf shift (1+ (* 2 (- shift-code 29)))
                    base (logior base (ash 1 (1- shift))))
              (setf shift (* 2 shift-code)))
          (make-encoded-cell-id-vector
           :deltas (encoded-uint-vector-init decoder 8)
           :base base
           :shift shift))))))

(defun encoded-cell-id-vector-decode (eciv)
  (declare (type encoded-cell-id-vector eciv))
  (let* ((size (encoded-cell-id-vector-size eciv))
         (result (make-array size :element-type '(unsigned-byte 64))))
    (dotimes (i size result)
      (setf (aref result i) (encoded-cell-id-vector-get eciv i)))))

(defun encode-cell-id-vector (v encoder)
  (declare (type (vector (unsigned-byte 64)) v)
           (type byte-vector encoder))
  (destructuring-bind (v-or v-and v-min v-max)
      (loop for cell-id across v
         with v-or = 0
         with v-and = -1
         do (setf v-or (logior v-or cell-id)
                  v-and (logand v-and cell-id))
         maximizing cell-id into v-max
         minimizing cell-id into v-min
         finally (return (list v-or v-and v-min v-max)))
    (let ((e-base 0)
          (e-base-len 0)
          (e-shift 0)
          (e-max-delta-msb 0))
      (when (> v-or 0)
        (setf e-shift (min 56 (dpb 0 (byte 1 0) (find-lsb-set v-or))))
        (unless (zerop (logand v-and (ash 1 e-shift)))
          (incf e-shift))
        (let ((e-bytes (1- (ash 1 65))))
          (dotimes (len 8)
            (let* ((t-base (mask-field (byte (* 8 len) (- 64 (* 8 len))) v-min))
                   (t-max-delta-msb (max 0 (1- (integer-length (ash (- v-max t-base)
                                                                    (- e-shift))))))
                   (t-bytes (+ len (* (length v) (1+ (ash t-max-delta-msb -3))))))
              (when (< t-bytes e-bytes)
                (setf e-base t-base
                      e-base-len len
                      e-max-delta-msb t-max-delta-msb
                      e-bytes t-bytes)))))
        (when (and (logbitp 0 e-shift)
                   (not (= (logand e-max-delta-msb 7) 7)))
          (decf e-shift)))
      (assert (<= e-base-len 7))
      (assert (<= e-shift 56))
      (encoder-ensure encoder (+ 2 e-base-len))

      (let ((shift-code (ash e-shift -1)))
        (when (logbitp 0 e-shift)
          (setf shift-code (min 31 (+ shift-code 29))))
        (encoder-put8 encoder (logior (ash shift-code 3)
                                      e-base-len))
        (when (= shift-code 31)
          (encoder-put8 encoder (ash e-shift -1))))

      (encode-uint-with-length
       (ash e-base (- (- 64 (* 8 (max 1 e-base-len))))) e-base-len encoder)

     (let ((deltas (make-array (length v) :element-type '(unsigned-byte 64))))
        (loop for cell-id across v
           for i from 0
           do (setf (aref deltas i)
                    (ash (- cell-id e-base) (- e-shift))))
        (encode-uint-vector deltas encoder)))))

;; encoded-point-vector
(defstruct encoded-point-vector
  (size 0 :type (unsigned-byte 32))
  (blocks nil :type encoded-string-vector)
  (base 0 :type (unsigned-byte 64))
  (level 0 :type (unsigned-byte 8))
  (have-exceptions nil :type boolean))

(defstruct cell-point
  (level 0 :type (unsigned-byte 8))
  (face 0 :type (unsigned-byte 8))
  (si 0 :type (unsigned-byte 32))
  (ti 0 :type (unsigned-byte 32)))

(defparameter +block-shift+ 4)
(defparameter +block-size+ (ash 1 +block-shift+))

(defparameter +exception+ -1)

(defstruct block-code
  (delta-bits 0 :type (signed-byte 32))
  (offset-bits 0 :type (signed-byte 32))
  (overlap-bits 0 :type (signed-byte 32)))

(defun bit-mask (n)
  (declare (type (integer 0 64) n))
  (if (zerop n) 0
      (dpb -1 (byte n 0) 0)))

(defun max-bits-for-level (level)
  (declare (type (integer 0 30) level))
  (+ (* 2 level) 3))

(defun base-shift (level base-bits)
  (declare (type (integer 0 30) level)
           (type (integer 0 64) base-bits))
  (max 0 (- (max-bits-for-level level) base-bits)))

(defun encode-point-vector-compact (points encoder)
  (declare (type (vector point) points)
           (type byte-vector encoder))
  (multiple-value-bind (level cell-points)
      (choose-best-level points)
    (when (< level 0)
      (return (encode-point-vector-fast points encoder)))
    (multiple-value-bind (values have-exceptions) (convert-cells-to-values cell-points level)
      (multiple-value-bind (base base-bits) (choose-base values level have-exceptions)
        (let ((num-blocks (ash (+ (length values) +block-size+ -1) (- +block-shift+)))
              (base-bytes (ash base-bits -3)))
          (encoder-ensure encoder (+ 2 base-bytes))
          (let ((last-block-count (- (length values) (* +block-size+ (1- num-blocks))))
                (blocks (make-string-vector-encoder))
                (exceptions (make-array 0 :element-type point :adjustable t)))
            (assert (>= last-block-count 0))
            (assert (<= last-block-count +block-size+))
            (assert (<= base-bytes 7))
            (assert (<= level 30))
            (encoder-put8 (logior +encoded-point-vector-cell-ids+
                                  (ash have-exceptions 3)
                                  (ash (1- last-block-count) 4)))
            (encoder-put8 (logior base-bytes
                                  (ash level 3)))
            (encode-uint-with-length (ash base (- (base-shift level base-bits))) base-bytes encoder)

            (loop for i from 0 to (length values) step +block-size+
               for block-size = (min +block-size+ (- (length values) i))
               for code = (get-block-code values i block-size base have-exceptions)
               do (let ((block (string-vector-encoder-add-via-encoder blocks))
                        (offset-bytes (ash (block-code-offset-bits code) -3))
                        (delta-nibbles (ash (block-code-delta-bits code) -2))
                        (overlap-nibbles (ash (block-code-overlap-bits code) -2)))
                    (encoder-ensure block (+ 1 offset-bytes (* delta-nibbles (ash +block-size+ -1))))
                    (assert (<= (- offset-bytes overlap-nibbles) 7))
                    (assert (<= overlap-nibbles 1))
                    (assert (<= delta-nibbles 16))
                    (encoder-put8 block (logior (- offset-bytes overlap-nibbles)
                                                (ash overlap-nibbles 3)
                                                (ash (1- delta-nibbles) 4)))

                    (let ((offset (1- (ash 1 65)))
                          (num-exceptions 0))
                      (dotimes (i block-size)
                        (let ((value (aref values (+ i j))))
                          (if (equalp value +exception+)
                              (incf num-exceptions)
                              (progn
                                (assert (>= value base))
                                (setf offset (min offset (- value base)))))))
                      (when (= num-exceptions block-size)
                        (setf offset))

                      (let ((offset-shift (- (block-code-delta-bits code)
                                             (block-code-overlap-bits code))))
                        (setf offset (logand offset (lognot (bit-mask offset-shift))))
                        (assert (= (zerop offset) (zerop offset-bytes)))
                        (when (> offset 0)
                          (encode-uint-with-length (ash offset (- offset-shift)) offset-bytes block))

                        (let ((delta-bytes (ash (1+ delta-nibbles) -1)))
                          (setf (fill-pointer exceptions) 0)
                          (dotimes (j block-size)
                            (let ((delta 0)
                                  (value (arerf values (+ i j))))
                              (if (equalp value +exception+)
                                  (progn
                                    (setf delta (length exceptions))
                                    (vector-push-extend (aref points (+ i j)) exceptions))
                                  (progn
                                    (assert (>= value (+ offset base)))
                                    (setf delta (- value (+ offset base)))
                                    (when have-exceptions
                                      (assert (<= delta (- (1- (ash 1 65)) +block-size+)))
                                      (incf delta +block-size+))))
                              (assert (<= delta (bit-mask (block-code-delta-bits delta))))
                              (when (and (logbitp delta-nibbles 0)
                                         (logbitp j 0))
                                ())
                              )))
                        ))))
))))))