;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; - NFS Share File - ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

(in-package :adage :use (list :lisp :util))

(declare-structure-type 3-vector x y z)

(defvar 3-vector-0 (make-3-vector 0 0 0))

(declare-primitive +
  (:inputs (x (Q ?n1 ?n2 ?n3)) (y (Q ?n1 ?n2 ?n3)))
  (:outputs (sum (Q ?n1 ?n2 ?n3)))
  (:output-procedure +))

(declare-primitive *
  (:inputs (x (Q ?n1 ?n2 ?n3)) (y (Q ?n4 ?n5 ?n6)))
  (:outputs (prod (Q ?n7 ?n8 ?n9)))
  (:output-procedure *))

(declare-primitive -
  (:inputs (x (Q ?n1 ?n2 ?n3)) (y (Q ?n1 ?n2 ?n3)))
  (:outputs (sum (Q ?n1 ?n2 ?n3)))
  (:output-procedure -))

(declare-primitive /
  (:inputs x y)
  (:outputs z)
  (:output-procedure /))

(declare-primitive sqrt
  (:inputs x)
  (:outputs y)
  (:output-procedure sqrt))

(declare-primitive square
  (:inputs x)
  (:outputs y)
  (:output-procedure square))

(defun square (x)
  (* x x))

(declare-primitive ignore
  (:inputs x)
  (:outputs)
  (:output-procedure noop))

(defun noop (x)
  (declare (ignore x))
  nil)

(defvar x-init-weight 0.3)

(defvar y-init-weight 0.7)



(declare-primitive inu-device
  (:inputs (measure-clock :clock))
  (:outputs (inu-pos 3-vector)
	    (inu-velocity 3-vector))
  (:clock-procedure measure-clock internally-update-inu)
  (:instance-maker make-inu-device)
  (:output-procedure updated-inu-output))

(defun make-inu-device () (let ((instance (gensym "INU-DEVICE")))
			    (setf (symbol-value instance) '((0 0 0) (0 0 0)))
			    instance))

(defun internally-update-inu (device)
  (setf (symbol-value device) (mapcar (lambda (list) (mapcar #'1+ list))
				      (symbol-value device))))

(defun updated-inu-output (device) (let ((val (symbol-value device)))
				     (values (apply #'make-3-vector (first val))
					     (apply #'make-3-vector (second val)))))

(declare-primitive gps-device
  (:inputs (measure-clock :clock))
  (:outputs (gps-pos 3-vector))
  (:clock-procedure measure-clock internally-update-gps)
  (:instance-maker make-gps-device)
  (:output-procedure updated-gps-output))

(defun make-gps-device () (let ((instance (gensym "GPS-DEVICE")))
			    (setf (symbol-value instance) '(1 1 1))
			    instance))
(defun internally-update-gps (device)
  (setf (symbol-value device) (mapcar #'1+ (symbol-value device))))
(defun updated-gps-output (device) (apply #'make-3-vector (symbol-value device)))

(declare-primitive max
  (:inputs x y)
  (:outputs z)
  (:output-procedure max))

(declare-primitive min
  (:inputs x y)
  (:outputs z)
  (:output-procedure min))

(declare-primitive cos
  (:inputs x)
  (:outputs z)
  (:output-procedure cos))

(declare-primitive sin
  (:inputs x)
  (:outputs z)
  (:output-procedure sin))

(declare-primitive atan
  (:inputs y x)
  (:outputs z)
  (:output-procedure atan))

(declare-primitive identity
  (:inputs x)
  (:outputs y)
  (:output-procedure id))

(defmacro id (exp)
  exp)