(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.00001d0
         (+ 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))))

(eval-when (:compile-toplevel :load-toplevel :execute)
  (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))
                                      #+nil
                                      (when (< ,prob 1.0d0)
                                        #> (,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
;;;

;;;
;;; 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))
  (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)
           (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))

;; 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)
  (orders nil :type (simple-array (unsigned-byte 16) (*)))
  (timer nil :type function :read-only t)
  (planets nil :type planets)
  (old-state nil :type (or null state)))

(defun clone-state (state)
  (let ((res (copy-state state)))
    (setf (st-planets res) (copy-seq (st-planets state))
          (st-stations res) (copy-seq (st-stations state))
          (st-orders res) (copy-seq (st-orders state)))
    res))

(defun simulated-annealing (state strategy anneal-fn)
  (let ((state (clone-state state))
        (best nil)
        (best-cost most-positive-double-float))
    (loop named hc do
      (when (terminate state strategy)
        #> ((st-turn state))
        (setf state best)
        (return-from hc best))
      (funcall anneal-fn state)
      ;; bestを更新
      (when (< (st-cost state) best-cost)
        (setf best (clone-state state)
              best-cost (st-cost state)))
      ;; ターン終了時の処理
      (on-ending-turn state strategy))))

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

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

(defun get-coord (state order)
  (let ((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 %calc-score-between-two-points (c-from c-to i-from i-to)
  (float
   (* (squared-euclid-distance c-from c-to)
      (if (%planet-id-p i-from)
          +alpha+
          1)
      (if (%planet-id-p i-to)
          +alpha+
          1))
   0d0))

(defun swap-orders! (state o1 o2)
  (rotatef (aref (st-orders state) o1)
           (aref (st-orders state) o2)))

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

(defun eval-state (state)
  (loop for i below (1- (orders-size state))
        for o1 = (aref (st-orders state) i)
        for o2 = (aref (st-orders state) (1+ i))
        for c1 = (get-coord state o1)
        for c2 = (get-coord state o2)
        for i1 = (%get-id-by-order state o1)
        for i2 = (%get-id-by-order state o2)
        sum (* (squared-euclid-distance c1 c2)
               (if (%planet-id-p i1)
                   +alpha+
                   1)
               (if (%planet-id-p i2)
                   +alpha+
                   1))
          into s
        finally (return (float s 0d0))))

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

;; ステーションの位置をずらす(y/x軸方向に最大±10)

(defstruct (neighbor (:constructor nil)))
(defstruct (change-station-position-event (:include change)
                                          (:conc-name cspe-))
  (station-id nil :type fixnum)
  (coord nil :type coord))

(defstruct (undo-change-station-position (:include undo-info)
                                         (:conc-name ucsp-))
  (cost-diff nil :type double-float)
  (station-id nil :type fixnum)
  (coord nil :type 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)

(defconstant +station-position-max-diff+ 10)

(defun emit-cspe (state)
  (let* ((station-id-offset (random +station-amount+))
         (station-id (+ +planet-amount+ station-id-offset))
         (c (aref (st-stations state) station-id-offset))
         (dy (%normalize (%rand (- +station-position-max-diff+) +station-position-max-diff+)
                         (- (y c))
                         (- +space-size+ (y c))))
         (dx (%normalize (%rand (- +station-position-max-diff+) +station-position-max-diff+)
                         (- (x c))
                         (- +space-size+ (x c))))
         (nc (make-coord (+ (y c) dy)
                         (+ (x c) dx))))
    (make-change-station-position-event :station-id station-id
                                        :coord nc)))

(defun %get-cost-diff-for-cspe (state cspe)
  ;; idは変化しない
  (with-accessors ((target-id cspe-station-id)
                   (new-coord cspe-coord)) cspe
    (let* ((current-coord (aref (st-stations state) (- target-id +planet-amount+)))
           (cost-diff 0d0))
      #+swank (assert (not (%planet-id-p target-id)))
      ;; スコアの差分を計算
      ;; stationの前後の距離が変化する
      (dotimes (order (orders-size state))
        (let* ((id (%get-id-by-order state order)))
          (when (= id target-id)
            (let ((prev-coord (get-coord state (1- order)))
                  (next-coord (get-coord state (1+ order)))
                  (prev-id (%get-id-by-order state (1- order)))
                  (next-id (%get-id-by-order state (1+ order))))
              (incf cost-diff
                    (+ (- (%calc-score-between-two-points new-coord
                                                          next-coord
                                                          id
                                                          next-id)
                          (%calc-score-between-two-points current-coord
                                                          next-coord
                                                          id
                                                          next-id))
                       (- (%calc-score-between-two-points new-coord
                                                          prev-coord
                                                          id
                                                          prev-id)
                          (%calc-score-between-two-points current-coord
                                                          prev-coord
                                                          id
                                                          prev-id))))))))
      cost-diff)))

(defun apply-cspe (state cspe)
  (with-accessors ((station-id cspe-station-id)
                   (new-coord cspe-coord)) cspe
    (let* ((station-id-offset (- station-id +planet-amount+))
           (current-coord (aref (st-stations state)
                                station-id-offset))
           (cost-diff (%get-cost-diff-for-cspe state cspe)))
      (setf (aref (st-stations state) station-id-offset)
            new-coord)
      (incf (st-cost state) cost-diff)
      (make-undo-change-station-position :cost-diff cost-diff
                                         :station-id station-id
                                         :coord current-coord))))

(defun undo-cspe (state ucsp)
  (with-accessors ((station-id ucsp-station-id)
                   (coord ucsp-coord)
                   (cost-diff ucsp-cost-diff)) ucsp
    (setf (aref (st-stations state) (- station-id +planet-amount+))
          coord)
    (decf (st-cost state)
          cost-diff)))

;; 2-opt

(defstruct (two-opt-change (:conc-name toc-)
                           (:include change))
  "2つの辺をつなぎかえる"
  (order1 nil :type fixnum)
  (order2 nil :type fixnum))
(defstruct (undo-two-opt (:conc-name uto-)
                         (:include undo-info))
  (score-diff nil :type double-float)
  (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)
          (return
            (make-two-opt-change :order1 id1
                                 :order2 id2)))))))

(defun apply-toc (state toc)
  ;; 内側はすべて反転
  (with-accessors ((o1 toc-order1)
                   (o2 toc-order2)) toc
    (let* ((o1-id (%get-id-by-order state o1))
           (o2-id (%get-id-by-order state o2))
           (o1-next-id (%get-id-by-order state (1+ o1)))
           (o2-next-id (%get-id-by-order state (1+ o2)))
           (from1 (get-coord state o1))
           (to1 (get-coord state (1+ o1)))
           (from2 (get-coord state o2))
           (to2 (get-coord state (1+ o2)))
           (score-diff (- (+ (%calc-score-between-two-points from1
                                                             from2
                                                             o1-id
                                                             o2-id)
                             (%calc-score-between-two-points to2
                                                             to1
                                                             o2-next-id
                                                             o1-next-id))
                          (+ (%calc-score-between-two-points from1 to1 o1-id o1-next-id)
                             (%calc-score-between-two-points from2 to2 o2-id o2-next-id))))
           (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 ((o1 uto-order1)
                   (o2 uto-order2)
                   (score-diff uto-score-diff)) 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))))

;; 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 id)
          (push i (gethash id res)))))
    res))

(defconstant +time-limit+ 0.8d0)

(defparameter *anneal-iteration-fn*
  (sa-tools:compile-anneal-fn
   ((:ratio 8.0d0
     :eval-fn #f(- (st-cost %))
     :event-fn emit-toc
     :apply-fn apply-toc
     :undo-fn undo-toc)
    (:ratio 1.0d0
     :eval-fn #f(- (st-cost %))
     :event-fn emit-cspe
     :apply-fn apply-cspe
     :undo-fn undo-cspe))
   :optimize #-swank t #+swank nil))

(defun solve (strategy start planets stations orders)
  (let ((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 (copy-seq stations)
                           :planets (copy-seq planets)
                           :orders (copy-seq orders))))
    (setf (st-cost state) (eval-state state))
    (simulated-annealing state
                         strategy
                         (lambda (state)
                           (funcall *anneal-iteration-fn*
                                    state
                                    ;; current-time
                                    (st-time state)
                                    ;; time-limit
                                    +time-limit+
                                    ;; start-temp
                                    1000d0
                                    ;; end-temp
                                    1d0)))))

(defun main ()
  (let* ((start (get-internal-real-time))
         (planets (read-planets))
         (stations (make-init-station-coords planets))
         (orders (coerce (append (list 0)
                                 ;; 惑星はそれぞれ1個用意する(惑星1は例外)
                                 (loop for i below +planet-amount+
                                       append (loop repeat 1 collect i))
                                 ;; ステーションはそれぞれ8個用意する
                                 (loop for i from +planet-amount+ below +all-amount+
                                       append (loop repeat 8 collect i))
                                 (list 0))
                         '(simple-array (unsigned-byte 16) (*))))
         (strategy (make-by-time-strategy :time-limit +time-limit+))
         ;; 並列で実行して良かったほうを採用する
         (solver-threads (loop repeat 2
                               collect (sb-thread:make-thread
                                        (lambda ()
                                          (solve strategy start planets stations orders))))))
    (let ((best (first
                 (sort (mapcar #'sb-thread:join-thread solver-threads)
                       #'<
                       :key #'st-cost))))
      (print-ans (st-stations best)
                 (map 'orders #f(rem % +all-amount+) (st-orders best))))))

#-swank (main)

;;;
;;; Debug
;;;

;; 00Raise 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*))))