(require 'asdf)

(push #P"/usr/local/asdf-install/site-systems/" asdf:*central-registry*)

(eval-when (:compile-toplevel :load-toplevel :execute)
  (handler-bind ((style-warning #'muffle-warning))
    (asdf:operate 'asdf:load-op 'vecto :verbose nil)))

(in-package #:vecto)
(defun sneakily-get-vecto-buffer ()
  (image-data *graphics-state*))
(export (find-symbol "SNEAKILY-GET-VECTO-BUFFER"))

(defpackage #:swarm-fft (:use #:cl))
(in-package #:swarm-fft)

(defun box-muller (&optional (random-state *random-state*))
  (let ((u1 (random 1.0 random-state))
	(u2 (random 1.0 random-state)))
    (let ((rr (sqrt (* -2.0 (log u1))))
	  (aa (* 2.0 pi u2)))
      (values (* rr (cos aa))
	      (* rr (sin aa))))))

(defun make-random-vector ( dims &optional (scale 1.0d0)
			                   (random-state *random-state*))
  (flet ((make-random-list ()
	   (subseq (loop :for ii :from 0 :below dims :by 2
		      :appending (multiple-value-bind (aa bb)
				     (box-muller random-state)
				   (list (* aa scale)
					 (* bb scale))))
		   0
		   dims)))
    (make-array (list dims) :element-type 'double-float
		:initial-contents (make-random-list))))

(defstruct bee
  (location (make-random-vector 5 2.0) :type (array double-float (5)))
  (velocity (make-random-vector 5 2.0) :type (array double-float (5))))

(defun make-random-bee ( &optional (random-state *random-state*) )
  (make-bee :location (make-random-vector 5 2.0 random-state)
	      :velocity (make-random-vector 5 0.25 random-state)))

(defun bee-amplitude ( bee )
  (* (aref (bee-location bee) 0) 0.15d0))
(defun bee-x ( bee )
  (aref (bee-location bee) 1))
(defun bee-y ( bee )
  (aref (bee-location bee) 2))
(defun bee-h ( bee )
  (aref (bee-location bee) 3))
(defun bee-k ( bee )
  (aref (bee-location bee) 4))

(declaim (ftype (function (bee) double-float)
		bee-amplitude
		bee-x
		bee-y
		bee-h
		bee-k))

(defun make-rgb-value (vv)
  (declare (type double-float vv))
  (coerce (min (max (round (the double-float (* vv 255.0d0))) 0) 255)
	  '(unsigned-byte 8)))

(declaim (ftype (function (double-float) (unsigned-byte 8)) make-rgb-value))

(defun hsv-to-rgb (hh ss vv)
  (declare (type double-float hh ss vv))
  (let ((angle (* (mod hh (the double-float (* 2.0d0 pi))) 180.0d0 (/ pi))))
    (declare (type double-float angle))
    (let ((ff (- (/ angle 60) (floor (/ angle 60)))))
      (let ((pp (* vv (- 1 ss)))
	    (qq (* vv (- 1 (* ff ss))))
	    (tt (* vv (- 1 (* (- 1 ff) ss)))))
	(case (mod (floor (/ angle 60)) 6)
	  (0 (values vv tt pp 1.0))
	  (1 (values qq vv pp 1.0))
	  (2 (values pp vv tt 1.0))
	  (3 (values pp qq vv 1.0))
	  (4 (values tt pp vv 1.0))
	  (5 (values vv pp qq 1.0))
	  (t (error "Bad mod angle..")))))))

(declaim (ftype (function (double-float double-float double-float)
			  (values double-float double-float double-float))
		hsv-to-rgb))

(defun normalize (nx ny nz)
  (declare (type double-float nx ny nz))
  (let ((scale (sqrt (+ (* nx nx) (* ny ny) (* nz nz)))))
    (declare (type double-float scale))
    (if (< scale 0.00001d0)
	(values 0.0d0 0.0d0 0.0d0)
	(values (/ nx scale) (/ ny scale) (/ nz scale)))))

(declaim (ftype (function (double-float double-float double-float)
			  (values double-float double-float double-float))
		normalize))

(defun make-normal (fx fy)
  (normalize fx fy -1.0d0))

(declaim (ftype (function (double-float double-float)
			  (values double-float double-float double-float))
		make-normal))

(defun calculate-color ( bees target lights xx yy )
  (declare (type double-float xx yy)
	   (type bee target))
  (let ((ta (bee-amplitude target))
	(tx (bee-x target))
	(ty (bee-y target))
	(th (bee-h target))
	(tk (bee-k target)))
    (declare (type double-float ta tx ty th tk))
    (labels ((calc ( bees &optional (f 0.0d0) (fx 0.0d0) (fy 0.0d0) )
	       (declare (optimize (speed 3))
			(type double-float f fx fy))
	       (cond
		 ((null bees) (values f fx fy))
		 (t (let ((pp (first bees)))
		      (declare (type bee pp))
		      (let ((aa (- (bee-amplitude pp) ta))
			    (ax (* 2.0d0 pi (+ (* xx (- (bee-x pp) tx))
					       (- (bee-h pp) th))))
			    (ay (* 2.0d0 pi (+ (* yy (- (bee-y pp) ty))
					       (- (bee-k pp) tk)))))
			(declare (type double-float aa ax ay))
			(let ((cx (cos ax))
			      (cy (cos ay))
			      (sx (sin ax))
			      (sy (sin ay)))
			  (declare (type double-float cx cy sx sy))
			  (calc (rest bees)
				(+ f (the double-float (* aa cx cy)))
				(+ fx (the double-float
					(* aa -2.0d0 pi (- (bee-x pp) tx)
					   sx cy)))
				(+ fy (the double-float
					(* aa -2.0d0 pi (- (bee-y pp) ty)
					   cx sy)))))))))))
      (multiple-value-bind (f fx fy)
	  (calc bees)
	(declare (type double-float f fx fy))
	(flet ((specular (nx ny nz)
		 (declare (type double-float nx ny nz)
			  (optimize (speed 3)))
		 (loop :for ll :in lights
		    :summing (let ((lx (first ll))
				   (ly (second ll))
				   (lz (third ll)))
			       (declare (type double-float lx ly lz))
			       (let ((dot (+ (the double-float (* nx lx))
					     (the double-float (* ny ly))
					     (the double-float (* nz lz)))))
				 (declare (type double-float dot))
				 (if (minusp dot)
				     (expt (- dot) (fourth ll))
				     0.0d0))) double-float)))
	  (declare (ftype (function (double-float double-float double-float)
				    double-float) specular))
	  (multiple-value-bind (nx ny nz)
	      (make-normal fx fy)
	    (multiple-value-bind (dr dg db)
		(hsv-to-rgb (* (abs f) 10.0d0) 0.9d0 (abs nz))
	      (let ((ss (* (specular nx ny nz) 0.3)))
		(values (make-rgb-value (+ dr ss))
			(make-rgb-value (+ dg ss))
			(make-rgb-value (+ db ss)))))))))))

(declaim (ftype (function (list bee list double-float double-float)
			  (values (unsigned-byte 8)
				  (unsigned-byte 8)
				  (unsigned-byte 8)))
		calculate-color))

(defun render-bees ( bees width height )
  (let* ((rr (/ (min width height) 8))
	 (center (complex (* rr 3.125)
			  (* rr 3.125))))
    (labels ((map-real-line-to-interval ( xx )
	       (let ((vv (* xx 1.0)))
		 (/ vv (sqrt (1+ (* vv vv))))))
	     (to-unscaled-x-y ( angle )
	       (let ((radians (* pi angle (/ 180.0))))
		 (complex (sin radians) (cos radians))))
	     (to-x-y ( value angle )
	       (let ((mag (* rr (map-real-line-to-interval value))))
		 (+ (* (+ mag rr rr) (to-unscaled-x-y angle))
		    center)))
	     (move-to ( pt )
	       (vecto:move-to (realpart pt) (imagpart pt)))
	     (line-to ( pt )
	       (vecto:line-to (realpart pt) (imagpart pt)))
	     (draw-line ( r1 a1 r2 a2 )
	       (let* ((p1 (to-x-y r1 a1))
		      (p2 (to-x-y r2 a2))
		      (rm (/ (+ r1 r2) 2))
		      (am (/ (+ a1 a2) 2))
		      (m-real (to-x-y rm am))
		      (m-line (/ (+ p1 p2) 2))
		      (delta (- m-real m-line))
		      (diff (sqrt (realpart (* delta (conjugate delta))))))
		 (unless (< diff 0.5)
		   (draw-line r1 a1 rm am)
		   (draw-line rm am r2 a2))
		 (line-to p2)))
	     (draw-bee ( bee target )
	       (let ((bb (bee-location bee))
		     (tt (bee-location target)))
		 (move-to (to-x-y (- (aref bb 0) (aref tt 0)) 0))
		 (draw-line (- (aref bb 0) (aref tt 0)) 0
			    (- (aref bb 1) (aref tt 1)) 72)
		 (draw-line (- (aref bb 1) (aref tt 1)) 72
			    (- (aref bb 2) (aref tt 2)) 144)
		 (draw-line (- (aref bb 2) (aref tt 2)) 144
			    (- (aref bb 3) (aref tt 3)) 216)
		 (draw-line (- (aref bb 3) (aref tt 3)) 216
			    (- (aref bb 4) (aref tt 4)) 288)
		 (draw-line (- (aref bb 4) (aref tt 4)) 288
			    (- (aref bb 0) (aref tt 0)) 360)
		 (vecto:close-subpath)
		 (vecto:stroke)
		 (loop :for ii :from 0 :below 5
		    :for angle = 0 :then (+ angle 72)
		    :do (let ((pt (to-x-y (- (aref bb ii) (aref tt ii))
					  angle)))
			  (vecto:centered-circle-path (realpart pt)
						      (imagpart pt)
						      3)
			  (vecto:fill-path)))))
	     (draw-pentagon ( radius )
	       (move-to (+ (* radius (to-unscaled-x-y 0)) center))
	       (loop :for ii :from 1 :below 5
		  :for angle = 72 :then (+ angle 72)
		  :do (line-to (+ (* radius (to-unscaled-x-y angle))
				  center)))
	       (vecto:close-subpath)))

      (vecto:set-rgba-fill 1.0 1.0 1.0 0.4)
      (vecto:centered-circle-path (realpart center) (imagpart center) (* rr 1))
      (vecto:centered-circle-path (realpart center) (imagpart center) (* rr 3))
      (vecto:even-odd-fill)

      (vecto:set-rgba-stroke 1.0 1.0 0.3 0.3)
      (vecto:set-line-width 1)
      (vecto:centered-circle-path (realpart center) (imagpart center) (* rr 2))
      (vecto:stroke)

      (vecto:set-rgba-fill 0.5 0.8 0.5 0.8)
      (vecto:set-rgba-stroke 1.0 1.0 1.0 0.3)
      (vecto:set-line-width 3)
      (draw-bee (first (rest bees)) (first bees))

      (vecto:set-rgba-fill 0.8 0.5 0.5 0.6)
      (vecto:set-rgba-stroke 0.0 0.0 0.0 0.5)
      (vecto:set-line-width 1)
      (mapc #'(lambda (bee) (draw-bee bee (first (rest bees))))
	    (rest (rest bees))))))

(defun make-image ( pathname bees lights &optional (width 512) (height 512) )
  (declare (type fixnum width height))
  (vecto:with-canvas (:width width :height height)
    (let ((buffer (vecto:sneakily-get-vecto-buffer))
	  (index 0))
      (declare (type (array (unsigned-byte 8) (*)) buffer))
      (loop :for jj :from 0 :below height
	 :do (loop :for ii :from 0 :below width
		:do (multiple-value-bind (rr gg bb)
			(calculate-color (rest (rest bees))
					 (first (rest bees))
					 lights
					 (/ (- ii (* width 0.5d0))
					    width 0.75d0)
					 (/ (- jj (* height 0.5d0))
					    width 0.75d0))
		      (declare (type (unsigned-byte 8) rr gg bb))
		      (setf (aref buffer (+ index 0)) rr
			    (aref buffer (+ index 1)) gg
			    (aref buffer (+ index 2)) bb
			    (aref buffer (+ index 3)) 255)
		      (incf index 4)))))
    (render-bees bees width height)
    (vecto:save-png pathname)))

(defvar +max-lead-acceleration+ 8.0)
(defvar +max-lead-velocity+ 12.0)
(defvar +max-acceleration+ 15.0)
(defvar +max-velocity+ 25.0)

(defun advance-bees ( bees elapsed )
  (labels ((subtract-with-copy (aa bb)
	     (let ((ret (copy-seq aa)))
	       (loop :for ii :from 0 :below (length aa)
		  :do (decf (aref ret ii) (aref bb ii)))
	       ret))
	   (dot (aa bb)
	     (loop :for ii :from 0 :below (length aa)
		:summing (* (aref aa ii) (aref bb ii))))
	   (mag (aa)
	     (sqrt (dot aa aa)))
	   (scale (aa ss)
	     (loop :for ii :from 0 :below (length aa)
		:do (setf (aref aa ii) (* (aref aa ii) ss)))
	     aa)
	   (increment (aa bb ss)
	     (loop :for ii :from 0 :below (length aa)
		:do (incf (aref aa ii) (* (aref bb ii) ss)))
	     aa)
	   (advance (pp target &optional (max-acc +max-acceleration+)
			          (max-vel +max-velocity+))
	     (let ((aa (subtract-with-copy (bee-location pp) target)))
	       (scale aa (* (- (mag aa)) max-acc))
	       (increment (bee-velocity pp) aa elapsed)
	       (when (> (mag (bee-velocity pp)) max-vel)
		 (scale (bee-velocity pp)
			(/ max-vel (mag (bee-velocity pp)))))
	       (increment (bee-location pp) (bee-velocity pp) elapsed))))
    ;; the first bee is the leader's goal
    (advance (second bees)
	     (bee-location (first bees))
	     +max-lead-acceleration+ +max-lead-velocity+)
    ;; when the leader gets too close to the goal, give him a new goal
    (when (< (mag (subtract-with-copy (bee-location (first bees))
				      (bee-location (second bees))))
	     0.5)
      (setf (first bees) (make-random-bee))
      (format t "~S~%" (bee-location (first bees))))

    ;; everyone else follows the leader
    (mapc #'(lambda (pp)
	      (advance pp (bee-location (second bees))))
	  (rest (rest bees)))
    bees))
	       

(ignore-errors
  (with-open-file (in #P"random-seed.lisp"
		      :direction :input)
    (let ((rs (read in nil nil)))
      (when rs
	(setf *random-state* rs)))))

(loop :for ii :from 1 :to 3600
   :with elapsed = (/ 1800.0)
   :for bees = (let ((orig (loop :for ii :from 1 :to 12
				:collecting (make-random-bee))))
		   (loop :for ii :from 1 :to 900
		      :do (advance-bees orig elapsed))
		   orig)
        :then (advance-bees bees elapsed)
   :with lights = (list (list 0.3d0 0.5d0 (sqrt 0.66d0) 35.0d0)
			(list -0.3d0 (- (sqrt 0.66d0)) 0.5d0 45.0d0)
			(list (sqrt 0.66d0) 0.3d0 0.5d0 15.0d0))
   :do (make-image (format nil "foo~4,'0D.png" ii)
		   bees
		   lights
		   640 360))

(with-open-file (out #P"random-seed.lisp"
		     :direction :output
		     :if-exists :supersede
		     :if-does-not-exist :create)
  (write *random-state* :stream out))