(declare-primitive flight-plan
  (:inputs (measure-clock :clock))
  (:outputs (plan-pos 3-vector)
	    (plan-next-pos 3-vector))
  (:clock-procedure measure-clock internally-update-plan)
  (:instance-maker make-plan)
  (:output-procedure plan-output))

(defstruct plan
  (time 0))

(defvar *airframe-clock-period* 1)

(defvar *lead-time* 15)

(defun internally-update-plan (device)
  (incf (plan-time device) *airframe-clock-period*))

(defun plan-output (device)
  (let ((time (plan-time device)))
    (values (plan-position time)
	    (plan-position (+ time *lead-time*)))))

(defvar *x-vel* (/ 2 60.0))
(defvar *y-vel* (/ 2 60.0))

(defvar *initial-angle* (- (/ 3.14159 4.0)))

(defvar *rotation-rate* (/ 3.14159 60.0))
  
(defun plan-position (time)
  (cond ((< time 60) (make-3-vector (+ 1.0 (* time *x-vel*))
				    (+ -3.0 (* time *y-vel*))
				    1000))
	((< time 120) (let ((x-y-angle (+ *initial-angle* (* (- time 60) *rotation-rate*)))
			    (z-angle (* 2 3.14159 (/ (- time 60) 60))))
			(make-3-vector (+ 2 (* 1.414 (cos x-y-angle)))
				       (* 1.414 (sin x-y-angle))
				       (+ 550 (* 450 (cos z-angle))))))
	(t (make-3-vector (+ 1 (* (- time 120) (- *x-vel*)))
			  (+ 1 (* (- time 120) (- *y-vel*)))
			  1000))))

(defun print-plan (plan-fun timeout)
  (dotimes (time timeout)
    (let ((vec (funcall plan-fun time)))
      (format t "~%~s ~s ~s" (3-vector-x vec) (3-vector-y vec) (3-vector-z vec)))))

(declare-structure-type sensor-data gps-pos ins-pos ins-vel)

(declare-primitive noise
  (:inputs (clock :clock))
  (:outputs noise)
  (:instance-maker make-noise-generator)
  (:clock-procedure clock update-noise-maker)
  (:output-procedure noise-output))

(defstruct noise-generator
  (noise 0))

(defun update-noise-maker (device)
  (setf (noise-generator-noise device) (random 1.0)))

(defun noise-output (device)
  (noise-generator-noise device))
    
(defvar 3d-zero (make-3-vector 0 0 0))

(declare-structure-type nav-data pos vel)