(in-package #:cl-user)

;;;
;;; Init
;;;

(eval-when (:compile-toplevel :load-toplevel :execute)
  ;; #+swank (declaim (optimize (speed 3) (safety 2)))
  #+swank (declaim (optimize (speed 0) (safety 3) (debug 3)))
  #-swank (declaim (optimize (speed 3) (safety 0) (debug 0)))
  #+swank (ql:quickload :rove :silent t)
  #-swank (declaim (sb-ext:muffle-conditions sb-ext:compiler-note))
  #-swank (sb-ext:disable-debugger))

;;;
;;; Reader Macros
;;;

(eval-when (:compile-toplevel :load-toplevel :execute)
  (set-dispatch-macro-character
   #\# #\f
   #'(lambda (stream c2 n)
       (declare (ignore c2 n))
       (let ((form (read stream t nil t)))
         `(lambda (&optional %) (declare (ignorable %)) ,form))))

  (set-dispatch-macro-character
   #\# #\>
   #'(lambda (stream c2 n)
       (declare (ignore c2 n))
       (let ((form (read stream t nil t)))
         (declare (ignorable form))
         #-swank nil
         #+swank (if (atom form)
                     `(format *error-output* "~a => ~a~&" ',form ,form)
                     `(format *error-output* "~a => ~a~&" ',form `(,,@form)))))))

;;;
;;; Libraries
;;;

;;;
;;; BOF
;;;

;; sa
;; https://ja.wikipedia.org/wiki/%E7%84%BC%E3%81%8D%E3%81%AA%E3%81%BE%E3%81%97%E6%B3%95?action=edit

(defpackage cp-library/simulated-annealing-tools
  (:use #:cl)
  (:nicknames #:sa-tools)
  (:export #:random-prob
           #:get-temp
           #:get-prob
           #:rough-exp
           #:with-timer
           #:compile-anneal-fn))

(in-package sa-tools)

(declaim (inline random-prob))
(defun random-prob (x)
  (declare (double-float x))
  (< (random 1.0d0) x))

(declaim (inline rough-exp))
(defun rough-exp (x)
  (declare (double-float x)
           (optimize (speed 3) (safety 0)))
  (flet ((%n-th (n)
           (/ (expt x n)
              (loop for m from 1 to n
                    with tmp of-type fixnum = 1 do (setf tmp (* tmp m))
                    finally (return (float tmp 0d0))))))
    (declare (inline %n-th))
    (max 0.001d0
         (+ 1d0
            (%n-th 1)
            (%n-th 2)
            (%n-th 3)))))

(declaim (inline get-temp))
(defun get-temp (start-temp end-temp current-time end-time)
  (+ start-temp
     (* (- end-temp start-temp)
        (/ current-time end-time))))

(declaim (inline get-prob))
(defun get-prob (score next-score temp)
  (rough-exp (/ (- next-score score)
                temp)))

(defmacro with-timer (get-time &body body)
  (let ((init (gensym)))
    `(let ((,init (get-internal-real-time)))
       (flet ((,get-time ()
                (/ (float
                    (- (get-internal-real-time)
                       ,init))
                   #.(float internal-time-units-per-second))))
         (declare (inline ,get-time)
                  (dynamic-extent #',get-time))
         ,@body))))

(defstruct (adjacent (:conc-name %get-))
  (ratio 0d0)
  (event-fn nil)
  (eval-fn nil)
  (apply-fn nil)
  (undo-fn nil))

;; TODO xorshift

(defmacro compile-anneal-fn (adjs &key optimize)
  "See example below.
  lambda-list of returned-fn : (state current-time end-time start-time end-time)"
  (sb-int:with-unique-names (state p start-temp end-temp end-time current-time)
    (let* ((adjs (mapcar (lambda (adj)
                           (apply #'make-adjacent adj))
                         adjs))
           (rs (mapcar #'%get-ratio adjs))
           (es (mapcar #'%get-event-fn adjs))
           (eval-fns (mapcar #'%get-eval-fn adjs))
           (as (mapcar #'%get-apply-fn adjs))
           (undo-fns (mapcar #'%get-undo-fn adjs))
           (all (reduce #'+ rs)))
      `(lambda (,state ,current-time ,end-time ,start-temp ,end-temp)
         (declare (double-float ,current-time ,end-time ,start-temp ,end-temp)
                  ,@(when optimize `((optimize (speed 3) (safety 0) (debug 0)))))
         (let ((,p (random ,all)))
           (cond
             ,@ (loop for rate in rs
                      for get-event in es
                      for eval-state in eval-fns
                      for apply-event in as
                      for undo in undo-fns
                      with s = 0d0
                      for (event score new-score undo-info temp prob) = (loop repeat 6 collect (gensym))
                      do (incf s rate)
                      collect `((< ,p ,s)
                                (let ((,event (,get-event ,state)))
                                  (when ,event
                                    (let* ((,score (,eval-state ,state))
                                           (,undo-info (,apply-event ,state ,event))
                                           (,new-score (,eval-state ,state))
                                           (,temp (get-temp ,start-temp ,end-temp ,current-time ,end-time))
                                           (,prob (get-prob ,score ,new-score ,temp)))
                                      (declare (double-float ,score ,new-score ,temp ,prob))
                                      (when (> (random 1.0d0) ,prob)
                                        (,undo ,state ,undo-info)))))))))))))

#+nil
(flet ((two-opt (state)
         (let ((i (random (length state)))
               (j (random (length state))))
           (list i j)))
       (eval-state (state)
         (float (length state) 0d0))
       (apply-two-opt-event (state event)
         (destructuring-bind (i j) event
           (rotatef (aref state i) (aref state j))
           event))
       (undo-two-opt-event (state undo-info)
         (destructuring-bind (i j) undo-info
           (rotatef (aref state i) (aref state j)))))
  (disassemble
   (compile-anneal-fn
    ((:ratio 1.0d0
      :event-fn two-opt
      :eval-fn eval-state
      :apply-fn apply-two-opt-event
      :undo-fn undo-two-opt-event))
    :optimize t)))

;;;
;;; EOF
;;;

;;;
;;; BOF
;;;

(defpackage randomized-heap
  (:use #:cl)
  (:nicknames #:rh)
  (:shadow #:count)
  (:export #:make-randomized-heap
           #:empty-p
           #:push!
           #:pop!
           #:peak
           #:count
           #:do-all-kvs
           #:prune!))

(in-package #:randomized-heap)

(eval-when (:compile-toplevel :load-toplevel :execute)
  (defvar *OPT*
    #+swank '(optimize (speed 3) (safety 2) (debug 3))
    #-swank '(optimize (speed 3) (safety 0) (debug 0))))

(declaim (inline %make-node make-randomized-heap))
(defstruct (node (:constructor %make-node)
                 (:conc-name %n-))
  (key nil :type fixnum)
  (value nil :type t)
  (children (make-array 2 :element-type '(or null node)
                          :initial-element nil)
   :type (simple-array (or null node) (2))))

(defstruct (randomized-heap (:constructor make-randomized-heap (&optional (compare #'sb-impl::two-arg-<) (key-fn #'identity))))
  (root nil :type (or null node))
  (count 0 :type (integer 0 #.most-positive-fixnum))
  (comparator compare :type (function (fixnum fixnum) boolean))
  (key-fn key-fn :type (function (t) fixnum)))

(defun count (heap)
  (randomized-heap-count heap))

(declaim (inline %direction))
(defun %direction ()
  (declare #.*OPT*)
  (random 2))

(defun meld (l r comparator)
  (declare #.*OPT*
           ((or null node) l r)
           ((function (fixnum fixnum) boolean) comparator))
  (cond
    ((or (null l)
         (null r))
     (or l r))
    ((funcall comparator
              (%n-key l)
              (%n-key r))
     ;; l is root
     (let ((dir (%direction)))
       (setf (aref (%n-children l) dir)
             (meld (aref (%n-children l) dir)
                   r
                   comparator))
       l))
    (t
     ;; r is root
     (let ((dir (%direction)))
       (setf (aref (%n-children r) dir)
             (meld l
                   (aref (%n-children r) dir)
                   comparator))
       r))))

(declaim (inline peak))
(defun peak (heap)
  (declare #.*OPT*)
  (let ((root (randomized-heap-root heap)))
    (values (%n-value root)
            (%n-key root))))

(defun empty-p (heap)
  (null (randomized-heap-root heap)))

(declaim (inline pop! push!))
(defun pop! (heap)
  (declare #.*OPT*)
  (when (empty-p heap)
    (error "heap is empty"))
  (decf (randomized-heap-count heap))
  (multiple-value-bind (value key)
      (peak heap)
    (let* ((children (%n-children (randomized-heap-root heap)))
           (l (aref children 0))
           (r (aref children 1)))
      (setf (randomized-heap-root heap)
            (meld l
                  r
                  (randomized-heap-comparator heap)))
      (values value key))))

(defun push! (heap value)
  (declare #.*OPT*)
  (let ((key (funcall (randomized-heap-key-fn heap)
                      value)))
    (incf (randomized-heap-count heap))
    (setf (randomized-heap-root heap)
          (meld (randomized-heap-root heap)
                (%make-node :key key
                            :value value)
                (randomized-heap-comparator heap))))
  heap)

(defmacro do-all-kvs ((key value heap) &body body &environment env)
  (let ((temp (gensym)))
    (multiple-value-bind (args argvs res setter accessor)
        (get-setf-expansion heap env)
      `(let ((,temp (make-randomized-heap (randomized-heap-comparator ,accessor)
                                          (randomized-heap-key-fn ,accessor))))
         (loop :until (empty-p ,accessor)
               :do (multiple-value-bind (,value ,key)
                       (pop! ,accessor)
                     (progn ,@body)
                     (push! ,temp ,value)))
         (let* (,@(mapcar #'list args argvs)
                (,(car res) ,temp))
           ,setter
           nil)))))

(defun prune! (heap cnt)
  (let ((new-root nil)
        (comparator (randomized-heap-comparator heap)))
    (dotimes (_ cnt)
      (multiple-value-bind (v k) (pop! heap)
        (setf new-root
              (meld new-root
                    (%make-node :key k :value v)
                    comparator))))
    (setf (randomized-heap-root heap)
          new-root)))

;;;
;;; EOF
;;;

;;;
;;; Macros
;;;

(in-package #:cl-user)

(defmacro awhen (test &body forms)
  `(let ((it ,test))
     (when it
       ,@forms)))

(defmacro while (test &body body)
  `(loop while ,test do (progn ,@body)))

(defmacro eval-always (&body body)
  `(eval-when (:compile-toplevel :load-toplevel :execute)
     ,@body))

;;;
;;; Body
;;;

(in-package #:cl-user)

(defmacro %expander-body (form cases)
  `(ecase ,form
     ,@(loop for (from . to) in cases
             collect `(,from ,to))))

(defmacro define-ecase-expander (name assoc-list)
  `(defmacro ,name (form)
     `(%expander-body ,form ,',assoc-list)))

(eval-always
  (defconstant +space-size+ 1000)
  (defconstant +planet-amount+ 100)
  (defconstant +station-amount+ 8)
  (defconstant +all-amount+ (+ +planet-amount+ +station-amount+))
  (defconstant +alpha+ 5))

(deftype coord () '(unsigned-byte 32))
(declaim (inline y x))
(defun make-coord (y x)
  #+swank (assert (<= 0 y +space-size+) () "y is out of range")
  #+swank (assert (<= 0 x +space-size+) () "x is out of range")
  (dpb y '#.(byte 16 16) x))
(defun y (c) (ldb (byte 16 16) c))
(defun x (c) (ldb (byte 16 0) c))

#+nil (y (make-coord 1000 0))
#+nil (x (make-coord 0 1000))

(declaim (inline squared-euclid-distance))
(defun squared-euclid-distance (c1 c2)
  (let* ((dy (- (y c1) (y c2)))
         (dx (- (x c1) (x c2))))
    (declare (fixnum dy dx))
    (+ (the fixnum (* dy dy))
       (the fixnum (* dx dx)))))

(deftype planets () '(simple-array coord (*)))

(deftype stations ()
  '(simple-array coord (*)))

(deftype orders ()
  "planetsはid0〜99, stationsはid100〜107
  最初と最後はid0でないといけない"
  '(simple-array fixnum (*)))

(defun read-planets ()
  (let ((n (read))
        (m (read))
        (planets (make-array +planet-amount+ :element-type 'coord)))
    (declare (ignore n m))
    (dotimes (i +planet-amount+)
      (setf (aref planets i)
            (make-coord (read) (read))))
    planets))

(defun print-ans (stations orders)
  (declare (orders orders)
           (stations stations))
  #>orders
  (princ
   (with-output-to-string (*standard-output*)
     ;; stations
     (sb-int:dovector (c stations)
       (format t "~a ~a~%" (y c) (x c)))
     (format t "~a ~%" (length orders))
     ;; orders
     (sb-int:dovector (o orders)
       (multiple-value-bind (type id)
           (let ((o (%truncate-id o)))
             (if (< o +planet-amount+)
                 (values 1 o)
                 (values 2 (- o +planet-amount+))))
         (format t "~a ~a~%" type (1+ id)))))))

(defun %get-center-of-gravity (ps)
  (when (null ps)
    (return-from %get-center-of-gravity
      ;; 中心らへんにランダムに置いておく
      (make-coord (+ 400 (random 201))
                  (+ 400 (random 201)))))
  (let ((gy (floor (loop for p in ps
                         sum (y p))
                   (length ps)))
        (gx (floor (loop for p in ps
                         sum (x p))
                   (length ps))))
    (make-coord gy gx)))

(defgeneric terminate (state strategy))
(defgeneric on-ending-turn (state strategy))

(defun %rand-nth (xs)
  (nth (random (length xs)) xs))

(defun simulated-annealing! (state strategy anneal-fn)
  (loop named hc do
    (when (terminate state strategy)
      (return-from hc))
    (funcall anneal-fn state)
    (on-ending-turn state strategy)))

;; state impl

(defconstant +max-ordres-size+ (* 1 +all-amount+))

(defstruct (state (:conc-name st-))
  (time nil :type double-float)
  (turn nil :type (integer 0 #.most-positive-fixnum))
  (cost nil :type double-float)
  (stations nil :type stations)
  ;; 各座標ごとに3つずつ入っている
  ;; 最初と最後の惑星1は含まない
  (orders nil :type (simple-array (unsigned-byte 16) (*)))
  (timer nil :type function :read-only t)
  (planets nil :type planets :read-only t)
  (orders-by-station-id (make-hash-table) :type hash-table)
  (old-state nil :type (or null state)))

(defun %planet-id-p (id)
  (< id +planet-amount+))

(defun %truncate-id (id)
  (rem id +all-amount+))

(defun %get-id-by-order (state order)
  (aref (st-orders state) order))

(defun get-coord (state order)
  (let ((id (%truncate-id (%get-id-by-order state order))))
    (if (%planet-id-p id)
        (aref (st-planets state) id)
        (aref (st-stations state) (- id +planet-amount+)))))

(defun %treat-cost (update state order)
  (let* ((now (get-coord state order))
         (prev (get-coord state (1- order)))
         (next (get-coord state (1+ order)))
         (planet? (%planet-id-p (%truncate-id (%get-id-by-order state order))))
         (prev-planet? (%planet-id-p (%truncate-id (%get-id-by-order state (1- order)))))
         (next-planet? (%planet-id-p (%truncate-id (%get-id-by-order state (1+ order))))))
    ;; 前後との関係
    (funcall update
             (+ (* (squared-euclid-distance prev now)
                   (if planet?
                       +alpha+
                       1)
                   (if prev-planet?
                       +alpha+
                       1))
                (* (squared-euclid-distance now next)
                   (if planet?
                       +alpha+
                       1)
                   (if next-planet?
                       +alpha+
                       1))))))

(defun %treat-cost-on-order-removal! (state order)
  (%treat-cost
   (lambda (s)
     #>s
     (decf (st-cost state)
           s))
   state
   order))

(defun %treat-cost-on-order-addition! (state order)
  (%treat-cost
   (lambda (s)
     #>s
     (incf (st-cost state)
           s))
   state
   order))

(defun move-station! (state id new-coord)
  (let* ((truncated-id (%truncate-id id))
         (orders (gethash truncated-id (st-orders-by-station-id state)))
         (current-coord (aref (st-stations state) (- truncated-id +planet-amount+)))
         (orders-new (cons new-coord
                           (remove current-coord orders :count 1))))
    ;; idは変化しない
    (setf (aref (st-stations state) (- truncated-id +planet-amount+)) new-coord
          (gethash truncated-id (st-orders-by-station-id state)) orders-new)))

(defun swap-orders! (state o1 o2)
  (let ((id1 (aref (st-orders state) o1))
        (id2 (aref (st-orders state) o2)))
    (unless (%planet-id-p id1)
      (setf (gethash id1 (st-orders-by-station-id state))
            (cons o2 (remove o1 (gethash id1 (st-orders-by-station-id state)) :count 1))))
    (unless (%planet-id-p id2)
      (setf (gethash id2 (st-orders-by-station-id state))
            (cons o1 (remove o2 (gethash id2 (st-orders-by-station-id state)) :count 1))))
    (rotatef (aref (st-orders state) o1)
             (aref (st-orders state) o2))))

(defun orders-size (state)
  (length (st-orders state)))

(defun eval-state (state)
  (float
   (loop for planet across (st-planets state)
         sum (loop for st across (st-stations state)
                   minimize (squared-euclid-distance planet st)))
   0d0))

(defun %clone-table (table)
  (let ((res (make-hash-table :test (hash-table-test table)
                              :size (hash-table-size table))))
    (maphash (lambda (key val)
               (setf (gethash key res) val))
             table)
    res))

(defstruct (change (:constructor nil)))
(defstruct (undo-info (:constructor nil)))

;; ステーションの場所を少し変える

(defstruct (neighbor (:constructor nil)))
(defstruct (change-station-position-event (:include change)
                                          (:conc-name cspe-))
  (station-ids nil :type (simple-array fixnum (*)))
  (cs nil :type (simple-array coord (*))))

(defstruct (undo-change-station-position (:include undo-info)
                                         (:conc-name ucsp-))
  (cost-diff nil :type fixnum)
  (station-ids nil :type (simple-array fixnum (*)))
  (cs nil :type (simple-array coord (*))))

#-swank (declaim (sb-ext:freeze-type change-station-position-event
                                     undo-change-station-position))

(defun %rand (min max)
  #+swank (assert (<= min max))
  (+ min (random (1+ (- max min)))))

(defun %normalize (x min max)
  (assert (<= min max))
  (max (min x max) min))

#+nil (%normalize -30 -20 20)
#+nil (%normalize -10 -20 20)
#+nil (%normalize 20 -20 20)
#+nil (%normalize 30 -20 20)
#+nil (%normalize 30 20 -20)

(defun emit-cspe (state)
  (let* ((amount (1+ (random 3)))
         (station-ids (make-array amount :element-type 'fixnum :initial-element -1))
         (cs (make-array amount :element-type 'coord)))
    (dotimes (i amount)
      (let* ((station-id-offset (loop for id-offset = (random +station-amount+)
                                      unless (find (+ id-offset +planet-amount+) station-ids)
                                        return id-offset))
             (station-id (+ +planet-amount+ station-id-offset))
             (c (aref (st-stations state) station-id-offset))
             (dy (%normalize (%rand -20 20)
                             (- (y c))
                             (- +space-size+ (y c))))
             (dx (%normalize (%rand -20 20)
                             (- (x c))
                             (- +space-size+ (x c))))
             (nc (make-coord (+ (y c) dy)
                             (+ (x c) dx))))
        (setf (aref station-ids i) station-id
              (aref cs i) nc)))
    (make-change-station-position-event :station-ids station-ids
                                        :cs cs)))

(defun apply-cspe (state cspe)
  (let ((station-ids (make-array (length (cspe-station-ids cspe))
                                 :element-type 'fixnum
                                 :initial-element -1))
        (cs (make-array (length (cspe-station-ids cspe))
                        :element-type 'coord)))
    (loop for s-id across (cspe-station-ids cspe)
          for nc across (cspe-cs cspe)
          for station-id-offset = (- s-id +planet-amount+)
          for current = (aref (st-stations state) station-id-offset)
          for i from 0
          do (setf (aref station-ids i) s-id
                   (aref cs i) current))
    (let ((cost-diff (loop for s-id across (cspe-station-ids cspe)
                           for new across (cspe-cs cspe)
                           for current across cs
                           for orders = (gethash s-id (st-orders-by-station-id state))
                           sum (loop for order in orders
                                     for c = (get-coord state order)
                                     sum (- (squared-euclid-distance new c)
                                            (squared-euclid-distance current c))))))
      (loop for s-id across (cspe-station-ids cspe)
            for nc across (cspe-cs cspe)
            for station-id-offset = (- s-id +planet-amount+)
            for current = (aref (st-stations state) station-id-offset)
            for i from 0
            do (setf (aref (st-stations state) station-id-offset) nc))
      (incf (st-cost state) cost-diff)
      (make-undo-change-station-position :cost-diff cost-diff
                                         :station-ids station-ids
                                         :cs cs))))

(defun undo-cspe (state ucsp)
  (loop for s-id across (ucsp-station-ids ucsp)
        for c across (ucsp-cs ucsp)
        for station-id-offset = (- s-id +planet-amount+)
        do (setf (aref (st-stations state) station-id-offset) c))
  (decf (st-cost state)
        (ucsp-cost-diff ucsp)))

(defun apply-cspe-and-eval (state cspe)
  (let* ((undo (apply-cspe state cspe))
         (res (float (- (st-cost state)) 0d0)))
    (undo-cspe state undo)
    res))

;; 2-opt

(defstruct (two-opt-change (:conc-name toc-)
                           (:include change))
  (order1 nil :type fixnum)
  (order2 nil :type fixnum))
(defstruct (undo-two-opt (:conc-name uto-)
                         (:include undo-info))
  (score-diff nil :type fixnum)
  (order1 nil :type fixnum)
  (order2 nil :type fixnum))

#-swank (declaim (sb-ext:freeze-type two-opt-change undo-two-opt))

(defun emit-toc (state)
  (let ((size (orders-size state)))
    (loop
      (let ((id1 (1+ (random (- size 2))))
            (id2 (1+ (random (- size 2)))))
        (unless (= id1 id2)
          (let ((obj-id1 (aref (st-orders state) id1))
                (obj-id2 (aref (st-orders state) id2)))
            (unless (= (%truncate-id obj-id1)
                       (%truncate-id obj-id2))
              (return
                (make-two-opt-change :order1 id1
                                     :order2 id2)))))))))

(defun apply-toc (state toc)
  ;; 内側はすべて反転
  (with-accessors ((o1 toc-order1)
                   (o2 toc-order2)) toc
    (let* ((from1 (get-coord state o1))
           (to1 (get-coord state (1+ o1)))
           (from2 (get-coord state o2))
           (to2 (get-coord state (1+ o2)))
           (score-diff (- (+ (squared-euclid-distance from1 from2)
                             (squared-euclid-distance to2 to1))
                          (+ (squared-euclid-distance from1 to1)
                             (squared-euclid-distance from2 to2))))
           (min (min o1 o2))
           (max (max o1 o2))
           (mid (floor (+ min max) 2)))
      (loop for u from (1+ min) to mid
            for v downfrom max
            when (< u v)
              do (swap-orders! state u v))
      (incf (st-cost state) score-diff)
      (make-undo-two-opt :score-diff score-diff
                         :order1 (toc-order1 toc)
                         :order2 (toc-order2 toc)))))

(defun undo-toc (state uto)
  (with-accessors ((score-diff uto-score-diff)
                   (o1 uto-order1)
                   (o2 uto-order2)) uto
    (let* ((min (min o1 o2))
           (max (max o1 o2))
           (mid (floor (+ min max) 2)))
      (loop for u from (1+ min) to mid
            for v downfrom max
            when (< u v)
              do (swap-orders! state u v))
      (decf (st-cost state) score-diff))))

(defun apply-toc-and-eval (state toc)
  (let* ((undo (apply-toc state toc))
         (res (float (- (st-cost state)) 0d0)))
    (undo-toc state undo)
    res))

;; strategy impl

(defstruct (strategy (:conc-name str-)
                     (:constructor nil)))

(defstruct (by-count-strategy (:conc-name bcs-)
                              (:include strategy))
  (cnt nil :type fixnum))

(defstruct (by-time-strategy (:conc-name bts-)
                             (:include strategy))
  (time-limit nil :type double-float))

(defmethod on-ending-turn ((state state) (_ strategy))
  (incf (st-turn state))
  (when (zerop (rem (st-turn state) 128))
    (setf (st-time state) (funcall (st-timer state)))))

(defmethod terminate ((state state) (bcs by-count-strategy))
  (>= (st-turn state) (bcs-cnt bcs)))

(defmethod terminate ((state state) (bts by-time-strategy))
  (>= (st-time state) (bts-time-limit bts)))

;;;

(defun %improve-with-k-means-method (planet-group-ids planets)
  (let* ((stations (loop for g-id across planet-group-ids
                         for p across planets
                         with ps = (make-array +station-amount+ :initial-element nil)
                         do (push p (aref ps g-id))
                         finally (return
                                   (map 'vector #'%get-center-of-gravity ps))))
         (planet-group-ids (map 'vector
                                (lambda (p)
                                  (loop for st across stations
                                        for i from 0
                                        with min-d = most-positive-fixnum
                                        with res = nil
                                        for d = (squared-euclid-distance st p)
                                        when (< d min-d)
                                          do (setf res i
                                                   min-d d)
                                        finally (return res)))
                                planets)))
    planet-group-ids))

(defun make-init-station-coords (planets)
  (let ((planet-group-ids (make-array +planet-amount+ :initial-element nil))
        (group-points (make-array +station-amount+ :initial-element nil)))
    (dotimes (i +planet-amount+)
      (setf (aref planet-group-ids  i)
            (random +station-amount+)))
    (dotimes (_ 20)
      (setf planet-group-ids (%improve-with-k-means-method planet-group-ids planets)))
    (dotimes (p-id +planet-amount+)
      (push (aref planets p-id)
            (aref group-points (aref planet-group-ids p-id))))
    (map 'stations
         #'%get-center-of-gravity
         group-points)))

(defconstant +inf+ #.(ash 1 60))
(deftype %costs () '(simple-array fixnum (* *)))

(defun %make-orders-by-station-id (orders)
  (let ((res (make-hash-table)))
    (dotimes (i (length orders))
      (let ((id (aref orders i)))
        (unless (%planet-id-p (%truncate-id id))
          (push i (gethash id res)))))
    res))

(defun main ()
  (let* ((start (get-internal-real-time))
         (planets (read-planets))
         (stations (make-init-station-coords planets))
         (orders (coerce (append (list 0)
                                 (loop for i below 100
                                       append (loop repeat 1 collect i))
                                 (loop for i from 100 below 108
                                       append (loop repeat 8 collect i))
                                 (list 0))
                         '(simple-array (unsigned-byte 16) (*))))
         (state (make-state :time 0.0d0
                            :turn 0
                            :cost 0d0
                            :timer (lambda ()
                                     (float
                                      (/ (- (get-internal-real-time)
                                            start)
                                         #.internal-time-units-per-second)
                                      0d0))
                            ;; :old-state nil
                            :stations stations
                            :planets planets
                            :orders orders
                            :orders-by-station-id (%make-orders-by-station-id orders)))
         (strategy (make-by-time-strategy :time-limit 0.8d0))
         (anneal! (sa-tools:compile-anneal-fn
                   ((:ratio 1.0d0
                     :eval-fn #f(- (st-cost state))
                     :event-fn emit-cspe
                     :apply-fn apply-cspe
                     :undo-fn undo-cspe)
                    (:ratio 10.0d0
                     :eval-fn #f(- (st-cost state))
                     :event-fn emit-toc
                     :apply-fn apply-toc
                     :undo-fn undo-toc))
                   :optimize #-swank t #+swank nil)))
    (setf (st-cost state) (eval-state state))
    (simulated-annealing! state
                          strategy
                          (lambda (state)
                            ;; (state current-time end-time start-temp end-temp)
                            (funcall anneal!
                                     state
                                     (st-time state)
                                     0.8d0
                                     1000d0
                                     5d0)))
    (print-ans (st-stations state)
               (map 'orders #f(rem % +all-amount+) (st-orders state)))))

#-swank (main)

;;;
;;; Debug
;;;

;; Raise error on warning at compile time

#+(and sbcl (not swank))
(eval-when (:compile-toplevel)
  (when (or (plusp sb-c::*compiler-warning-count*)
            sb-c::*undefined-warnings*)
    (error "compiler-error-count:~a, undefined warnings:~a"
           sb-c::*compiler-warning-count*
           sb-c::*undefined-warnings*)))

#+swank
(defun run-sample (infile outfile &optional (out *standard-output*) (vis t))
  (let ((args (append
               (list "tester.dll" "judge" "-i" infile "-o" outfile)
               (when vis (list "-v" "vis.png")))))
    (with-open-file (*standard-input* infile :direction :input)
      (with-open-file (*standard-output* outfile :direction :output
                                                 :if-exists :supersede)
        (main))
      (sb-ext:run-program "dotnet" args
                          :output out
                          :search t
                          :error *error-output*))))