(load "/Applications/actr6-1-3/load-act-r-6.lisp")
(require :dbus)
(use-package :dbus)

(defvar *pressedkey* "")

(defclass panda-screen ()
    ((subviews :accessor subs :initform nil)
     (panda-string :accessor panda-string :initform nil)))

;D-Bus object to be called from Python
(defvar *inst* (make-instance 'panda-screen))

;D-Bus session bus
(defvar *bus* (make-dbus-bus :session))

(dbus-bus-request-name *bus* "edu.rpi.cogsci.destem.panda")

(dbus-add-callback *bus* *inst* "/" "edu.rpi.cogsci.destem.panda")

;struct to hold info from Python. This will become the device
(defclass incoming-string ()
    ((ralph-x :accessor ralph-x :initform 0)
     (ralph-y :accessor ralph-y :initform 0)
     (ralph-angle :accessor ralph-angle :initform 0)
     (cane-x :accessor cane-x :initform 0)
     (cane-y :accessor cane-y :initform 0)))
     
(defclass game-token ()
    ((x :accessor x :initform 0 :initarg x)
     (y :accessor y :initform 0 :initarg y)
     (color :accessor color :initform 0)
     (height :accessor height :initform 20)
     (width :accessor width :initform 20)
     (kind :accessor kind :initarg :kind)))
     
(defclass ralph (game-token)
    ()
    (:default-initargs :kind 'ralph))
    
(defclass cane (game-token)
    ()
    (:default-initargs :kind 'cane))

(defmethod panda-screen-introspect ((self panda-screen))
  (format *standard-output* "Introspecting~%")
  (dbus::slurp-file "introspection2.xml"))
    
(defmethod panda-screen-visicon ((self panda-screen))
    (apply #'concatenate 'string (mapcar 'return-icon-feature (visicon-chunks (get-module :vision)))))

(defmethod panda-screen-process ((self panda-screen)
                                  (r-x number)
                                  (r-y number)
                                  (r-a number)
                                  (c-x number)
                                  (c-y number))
    (setf (ralph-x (panda-string *inst*)) r-x)
    (setf (ralph-y (panda-string *inst*)) r-y)
    (setf (ralph-angle (panda-string *inst*)) r-a)
    (setf (cane-x (panda-string *inst*)) c-x)
    (setf (cane-y (panda-string *inst*)) c-y)
)

;return list of chunks of type visual-location
;this is what constitutes the visicon
(defmethod build-vis-locs-for ((device panda-screen) vis-mod) 
    (let ((feats nil))
        (remove-if #'not ;if the individual component functions return nil, don't include them in the list of visual-location chunks
            (dolist (x (subs device) feats)
                (push (build-vis-locs-for x vis-mod) feats)))))
                
(defmethod build-vis-locs-for ((obj ralph) vis-mod)
    (let ((vl (car (define-chunks-fct `((isa visual-location
                                          color ,(color obj)
                                          screen-x ,(ralph-x (panda-string *inst*))
                                          screen-y ,(ralph-y (panda-string *inst*))
                                          height ,(height obj)
                                          width ,(width obj)
                                          kind ,(kind obj)
                                          value ,(ralph-angle (panda-string *inst*))))))))
                                          
        (setf (chunk-visual-object vl) obj)
        
        vl))
        
(defmethod build-vis-locs-for ((obj cane) vis-mod)
    (let ((vl (car (define-chunks-fct `((isa visual-location
                                          color ,(color obj)
                                          screen-x ,(cane-x (panda-string *inst*))
                                          screen-y ,(cane-y (panda-string *inst*))
                                          height ,(height obj)
                                          width ,(width obj)
                                          kind ,(kind obj)
                                          value ,(kind obj)))))))

        (setf (chunk-visual-object vl) obj)

        vl))
                    
;called when model moves attention to a visual feature
;given the device, and a chunk of type visual-location
;returns corresponding chunck of type visual-object        
(defmethod vis-loc-to-obj ((token game-token) vis-loc) 
    (let ((new-object (car (define-chunks-fct `((isa ,(chunk-slot-value-fct vis-loc 'kind)))))))
       (fill-default-vis-obj-slots new-object vis-loc)))
    
;use add-screen-object and delete-screen-object to update the visicon?

(defmethod device-handle-keypress ((device panda-screen) key)
  (case key
    (#\d (setf *pressedkey* "d"))
    (#\a (setf *pressedkey* "a"))
    (#\Space (setf *pressedkey* "space"))))
    
    
(defmethod get-mouse-coordinates ((device panda-screen))
    (vector 0 0))

(defmethod device-handle-click ((device panda-screen))
    nil)
    
(defmethod device-speak-string ((device panda-screen) stringÅ)
    nil)
    
(defmethod cursor-to-vis-loc ((device panda-screen))
    nil)

(defmethod device-update ((self panda-screen) time)
  (progn 
    (dbus-bus-pump *bus*)
    (proc-display)))
    ;;;;(print "refresh screen!")))
            
(defun return-icon-feature (chunk)
  (format nil "(~3D ~3D)~11T~A~17T~A~32T~S~50T~A~66T~A~%"
                  (chunk-slot-value-fct  chunk 'screen-x) 
                  (chunk-slot-value-fct  chunk 'screen-y) 
                  (feat-attended chunk (get-module :vision))
                  (chunk-slot-value-fct  chunk 'kind) 
                  (if (null (chunk-real-visual-value chunk))
                      (chunk-slot-value-fct  chunk 'value) 
                    (chunk-real-visual-value chunk))
                  (chunk-slot-value-fct  chunk 'color) 
                  (chunk-visicon-entry chunk)))
                  
    
(defun fly ()

    (reset)

    (push (make-instance 'ralph) (subs *inst*))
    (push (make-instance 'cane) (subs *inst*))
    (setf (panda-string *inst*) (make-instance 'incoming-string))

    (install-device *inst*)
    (proc-display)
    (print-visicon)

    ;(run-until-condition (lambda () nil) :real-time t)
    (run-until-condition (lambda () nil))
)

(clear-all)

(define-model psf-cmu

(sgp :v nil :visual-movement-tolerance 10.0) ;trace-detail high

(chunk-type standard-flight-pattern state shipx shipy shipangle targetangle)
(chunk-type (ralph-feature (:include visual-location)))
(chunk-type (cane-feature (:include visual-location)))
(chunk-type (ralph (:include visual-object)))
(chunk-type (cane (:include visual-object)))
(define-chunks (ralph isa chunk))
(define-chunks (cane isa chunk))

(add-dm
 (first-goal ISA standard-flight-pattern state begin))

(p start
  =goal>
    ISA          standard-flight-pattern
    state        begin
==>
  =goal>
    state        attend-ralph
  +visual-location>
    ISA          visual-location
    kind         ralph
)

(p attend-to-nothing
  =goal>
    ISA          standard-flight-pattern
    state        attend-ralph
  =visual-location>
    ISA          visual-location
    kind         ralph
    value        nil
==>
  =goal>
    state        hold
)

(p holding-pattern
  =goal>
    ISA          standard-flight-pattern
    state        hold
==>
  =goal>
    state begin
)

(p attend-ralph
  =goal>
    ISA          standard-flight-pattern
    state        attend-ralph
  =visual-location>
    ISA          visual-location
    kind         ralph
  - value        nil
  ?visual>
    state        free
==>
  =goal>
    state        begin
)

(goal-focus first-goal)
)