;;; -*- mode: Lisp; Syntax: Common-Lisp; -*-
;;;
;;; Copyright (c) 2009 by the authors.
;;;
;;; See LICENCE for details.

(in-package :hu.dwim.dises.test)

(defvar *mobility*)

(defstruct mobility
  (simulation-time nil :type (or null simulation-time))
  (position nil :type (or null number 2d))
  (continuation nil :type (or null function)))

(defmacro mobility (&body forms)
  `(bind ((mobility (make-mobility)))
     (setf (mobility-continuation mobility)
           (lambda (&optional value)
             (declare (ignore value))
             (with-call/cc
               (bind ((*mobility* mobility))
                 ,@forms))))
     mobility))

(defun set-position (2d &optional (simulation-time (simulation-time-of *simulator*)))
  (setf (mobility-position *mobility*) 2d)
  (setf (mobility-simulation-time *mobility*) simulation-time))

(defun get-position (&optional (mobility *mobility*))
  (funcall (mobility-continuation mobility))
  (mobility-position mobility))

(defun jump-to (x y)
  (set-position (2d x y)))

;; TODO: capture special variables automatically in the continuation
(defun/cc stop ()
  (let/cc k
    (bind ((mobility *mobility*)
           (k* (lambda (&optional value)
                 (let ((*mobility* mobility))
                   (funcall k value)))))
      (setf (mobility-continuation mobility) k*)
      k*)))

(defun/cc move-to (x y speed)
  (iter (with destination = (2d x y))
        (for time-delta = (hu.dwim.dises::simulation-time- (simulation-time-of *simulator*)
                                                           (mobility-simulation-time *mobility*)))
        (for position = (mobility-position *mobility*))
        (for distance = (* speed time-delta))
        (for vector = (- destination position))
        (for total-distance = (2d-length vector))
        (if (<= distance total-distance)
            (progn
              (set-position (+ position
                               (* distance
                                  (2d-normalize vector))))
              (stop))
            (progn
              (set-position destination (hu.dwim.dises::simulation-time+ (mobility-simulation-time *mobility*)
                                                                         (/ total-distance speed)))
              (finish)))))

(defun print-position (mobility)
  (format t "~%Position at ~A is ~A" (simulation-time-of *simulator*) (get-position mobility)))

(deftest test/mobility ()
  (run-with-new-simulator
    (schedule :now
      (iter (with m = (mobility
                        (jump-to 0 0)
                        (iter (repeat 2)
                              (move-to 200 200 10)
                              (move-to 100 100 5))
                        (move-to 0 0 100)))
            (print-position m)
            (wait 0.1)))
    (finish-at 100)))