;; Calculate 3-body problem (later: generalize to n-body)

;; General helpers

(defun unique-pairs (up-to)
  "Generate all unique combinations of numbers from 0 to up-to - 1 without (n,n) pairs. I.e., (1,2), (1, 3), ..., (2, 3), (2, 4), ..."
  (let ((result '()))
    (loop for i from 0 to (1- up-to) do
	 (loop for j from (1+ i) to (1- up-to) do
	      (push (list i j) result)))
    result))

;; VEC impl

(defstruct vec
  (x 0 :type number)
  (y 0 :type number)
  (z 0 :type number))

(defmacro for-all-vec-components ((vec var) &body body)
  `(progn ,@(loop for C in (list 'vec-x 'vec-y 'vec-z) collect
       `(let ((,var (,C ,vec))) ,@body))))

;(for-all-vec-components ((body-pos *tbody1*) comp) (format t "component: ~a~%" comp))

(defun norm-of (vec)
  (sqrt (+ (expt (vec-x vec) 2) (expt (vec-y vec) 2) (expt (vec-z vec) 2))))

(defun real-product (vec real)
  (make-vec :x (* real (vec-x vec))
	    :y (* real (vec-y vec))
	    :z (* real (vec-z vec))))

(defun vec-sum (&rest vecs)
  (flet ((sum (list) (reduce #'+ list)))
    (make-vec :x (sum (mapcar #'vec-x vecs))
	      :y (sum (mapcar #'vec-y vecs))
	      :z (sum (mapcar #'vec-z vecs)))))

;; BODY impl

(defstruct body
  (mass 0 :type number)
  (pos (make-vec) :type vec)
  (vel (make-vec) :type vec))

(defparameter *tbody1*
  (make-body :mass 20000000000
	     :pos (make-vec :x 0l0 :y 0l0 :z 0l0)
	     :vel (make-vec :x 0.005 :y 0 :z 0)))
(defparameter *tbody3*
  (make-body :mass 10000000
	     :pos (make-vec :x 2500l0 :y 3000l0 :z -200l0)
	     :vel (make-vec :x 0.01 :y 0 :z 0)))
(defparameter *tbody2*
  (make-body :mass 10000000
	     :pos (make-vec :x -2500l0 :y -3000l0 :z 500l0)
	     :vel (make-vec :x -0.01 :y 0 :z 0)))

(defconstant GRAVCONST 6.67428l-11)

(defun direction-between (body1 body2)
  (let* ((p1 (body-pos body1))
	(p2 (body-pos body2))
	(dx (- (vec-x p2) (vec-x p1)))
	(dy (- (vec-y p2) (vec-y p1)))
	(dz (- (vec-z p2) (vec-z p1))))
    (make-vec :x dx :y dy :z dz)))

(defun distance-between (body1 body2) (norm-of (direction-between body1 body2)))

(defun gravitational-force (body1 body2)
  (let* ((direction (direction-between body1 body2))
	 (dist (norm-of direction))
	 (unity-direction (real-product direction (/ 1 dist)))
	 (force (/ (* GRAVCONST (body-mass body1) (body-mass body2))
		   (expt dist 2)))
	 (directed-force (real-product unity-direction force)))
	 directed-force))

(defun accelerate-body (body force delta-t)
  (let* ((accel (real-product force (/ 1 (body-mass body))))
	 (delta-s-by-vel (real-product (body-vel body) delta-t))
	 (delta-s-by-accel (real-product accel (* 1/2 (expt delta-t 2))))
	 (delta-v-by-accel (real-product accel delta-t)))
    (setf (body-pos body) (vec-sum (body-pos body) delta-s-by-vel delta-s-by-accel))
    (setf (body-vel body) (vec-sum (body-vel body) delta-v-by-accel))
    body))

(defun calculate-body-step (bodies-vec delta-t)
  (loop for pair in (unique-pairs (length bodies-vec)) do
       (let* ((b1 (first pair))
	      (b2 (second pair))
	      (force-2-on-1 (gravitational-force (aref bodies-vec b1) (aref bodies-vec b2)))
	      (force-1-on-2 (real-product force-2-on-1 -1)))
	 (accelerate-body (aref bodies-vec b1) force-2-on-1 delta-t)
	 (accelerate-body (aref bodies-vec b2) force-1-on-2 delta-t))))

(defparameter *bodies-state-output* t)
(defun output-bodies-state (bodies-vec)
  (flet ((body-state (body)
	   (format *bodies-state-output* "~,3f, ~,3f, ~,3f"
		   (vec-x (body-pos body))
		   (vec-y (body-pos body))
		   (vec-z (body-pos body)))))
    (loop for body across bodies-vec do
	 (body-state body)
	 (format *bodies-state-output* ", "))
    (format *bodies-state-output* "~%")))