(defun make-pair (value weight) (cons value weight))
(defun get-value (pair) (car pair))
(defun get-weight (pair) (cdr pair))

(defun get-value-count-pairs (values)
  (let ((sorted (sort (copy-list values) #'<)))
    (loop for x in (rest sorted)
       with val = (first sorted)
       with cnt = 1
       if (< val x) collect (make-pair val cnt) into res and do (setf cnt 1 val x)
       else do (incf cnt)
       finally (return (append res (list (make-pair val cnt)))))))

(defun calc-weightened (pairs)
  (loop for (value . weight) in pairs
     summing weight into acc-weight
     summing (* value weight) into acc-weightened
     collect (make-pair acc-weightened acc-weight)))

(defun jenks-fisher (k values)
  (declare (optimize (debug 2)))
  (when (> k 1)
    (let* ((pairs (get-value-count-pairs values))
           (m (length pairs))
           (buf-size (- m (1- k)))
           (weightened (calc-weightened pairs))
           (prev-ssm (make-array buf-size))
           (cur-ssm (make-array buf-size))
           (cb (make-array (list (1- k) buf-size)))
           (compl 1))
      (labels ((get-wv (b e)
                 (assert (not (zerop b)))
                 (assert (<= b e))
                 (assert (< e m))
                 (- (get-value (elt weightened e)) (get-value (elt weightened (1- b)))))
               (get-w (b e)
                 (assert (not (zerop b)))
                 (assert (<= b e))
                 (assert (< e m))
                 (- (get-weight (elt weightened e)) (get-weight (elt weightened (1- b)))))
               (get-ssm (b e)
                 (/ (expt (get-wv b e) 2) (get-w b e)))
               (calc-ssm (p i)
                 (+ (elt prev-ssm p) (get-ssm (+ p compl) (+ i compl))))
               (find-max-break-index (i bp ep)
                 (assert (< bp ep))
                 (assert (<= bp i))
                 (assert (<= ep (1+ i)))
                 (assert (< i buf-size))
                 (assert (<= ep buf-size))
                 (let ((found-point bp)
                       (max-ssm (calc-ssm bp i)))
                   (loop for point from (1+ bp) below ep
                      for ssm = (calc-ssm point i)
                      when (> ssm max-ssm)
                      do (setf max-ssm ssm found-point point))
                   (setf (elt cur-ssm i) max-ssm)
                   found-point))
               (calc-range (bi ei bp ep)
                 (assert (<= bp bi ei))
                 (assert (<= ep ei))
                 (unless (= bi ei)
                   (assert (< bp ep))
                   (let* ((mi (floor (+ bi ei) 2))
                          (mp (find-max-break-index mi bp (min ep (1+ mi)))))
                     (assert (<= bp mp mi))
                     (assert (< mp ep))
                     (calc-range bi mi bp (min mi (1+ mp)))
                     (setf (aref cb (1- compl) mi) mp)
                     (calc-range (1+ mi) ei mp ep)))))
        ;; Init first prev-ssm
        (loop for i below buf-size
           for (value . weight) in weightened
           do (setf (elt prev-ssm i)
                    (/ (expt value 2) weight)))
        ;; Calc all
        (dotimes (i (- k 2))
          (setf compl (1+ i))
          (calc-range 0 buf-size 0 buf-size)
          (rotatef prev-ssm cur-ssm))
        (cons (get-value (elt pairs 0))
              (reverse
               (loop for idx from (1- k) downto 1
                  with last-bi = (find-max-break-index (1- buf-size) 0 buf-size)
                  collect (get-value (elt pairs (+ last-bi idx)))
                  do (assert (< last-bi buf-size))
                  when (> idx 1)
                  do (setf last-bi (aref cb (1- idx) last-bi)))))))))