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

(defvar *joyx* 0)
(defvar *joyy* 0)
(defvar *trigger* 0)
(defvar *pressedkey* "")

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

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

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

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

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

;struct to hold info from Python. This will become the device
(defclass incoming-string ()
    ((ship-x :accessor ship-x :initform 0)
     (ship-y :accessor ship-y :initform 0)
     (ship-angle :accessor ship-angle :initform 0)
     (mine-exist :accessor mine-exist :initform "n")
     (mine-x :accessor mine-x :initform 0)
     (mine-y :accessor mine-y :initform 0)
     (IFF-letter :accessor IFF-letter :initform "")
     (shell-exist :accessor shell-exist :initform "n")
     (shell-x :accessor shell-x :initform 0)
     (shell-y :accessor shell-y :initform 0)
     (bonus-exist :accessor bonus-exist :initform "n")
     (bonus-symbol :accessor bonus-symbol :initform "")))
     
(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 ship (game-token)
    ((angle :accessor angle :initform 0))
    (:default-initargs :kind 'ship))
    
(defclass mine (game-token)
    ()
    (:default-initargs :kind 'mine))
    
(defclass IFF (game-token)
    ((letter :accessor letter :initform ""))
    ;(:default-initargs :kind 'IFF :x 400 :y 400))
    (:default-initargs :kind 'IFF))
    
(defclass shell (game-token)
    ()
    (:default-initargs :kind 'shell))
    
(defclass bonus (game-token)
    ((symb :accessor symb :initform ""))
    (:default-initargs :kind 'bonus))

(defmethod pygame-screen-introspect ((self pygame-screen))
  (format *standard-output* "Introspecting~%")
  (dbus::slurp-file "introspection.xml"))
  
(defmethod pygame-screen-getjoystick ((self pygame-screen))
    (format nil "~a ~a" *joyx* *joyy*))
    
(defmethod pygame-screen-visicon ((self pygame-screen))
    (apply #'concatenate 'string (mapcar 'return-icon-feature (visicon-chunks (get-module :vision)))))

(defmethod pygame-screen-process ((self pygame-screen)
                                  (s-x number)
                                  (s-y number)
                                  (s-a number)
                                  (m-e string)
                                  (m-x number)
                                  (m-y number)
                                  (i-l string)
                                  (sh-e string)
                                  (sh-x number)
                                  (sh-y number)
                                  (b-e string)
                                  (b-s string))
    (setf (ship-x (pygame-string *inst*)) s-x)
    (setf (ship-y (pygame-string *inst*)) s-y)
    (setf (ship-angle (pygame-string *inst*)) s-a)
    (setf (mine-exist (pygame-string *inst*)) m-e)
    (setf (mine-x (pygame-string *inst*)) m-x)
    (setf (mine-y (pygame-string *inst*)) m-y)
    (setf (IFF-letter (pygame-string *inst*)) i-l)
    (setf (shell-exist (pygame-string *inst*)) sh-e)
    (setf (shell-x (pygame-string *inst*)) sh-x)
    (setf (shell-y (pygame-string *inst*)) sh-y)
    (setf (bonus-exist (pygame-string *inst*)) b-e)
    (setf (bonus-symbol (pygame-string *inst*)) b-s)
)

;return list of chunks of type visual-location
;this is what constitutes the visicon
(defmethod build-vis-locs-for ((device pygame-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 ship) vis-mod)
    (let ((vl (car (define-chunks-fct `((isa visual-location
                                          color ,(color obj)
                                          screen-x ,(ship-x (pygame-string *inst*))
                                          screen-y ,(ship-y (pygame-string *inst*))
                                          height ,(height obj)
                                          width ,(width obj)
                                          kind ,(kind obj)
                                          value ,(ship-angle (pygame-string *inst*))))))))
                                          
        (setf (chunk-visual-object vl) obj)
        
        vl))
        
(defmethod build-vis-locs-for ((obj IFF) vis-mod)
    (let ((vl (car (define-chunks-fct `((isa visual-location
                                          color ,(color obj)
                                          screen-x ,(x obj)
                                          screen-y ,(y obj)
                                          height ,(height obj)
                                          width ,(width obj)
                                          kind ,(kind obj)
                                          value ,(IFF-letter (pygame-string *inst*))))))))

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

        vl))
        
(defmethod build-vis-locs-for ((obj mine) vis-mod)
    (if (string= "n" (mine-exist (pygame-string *inst*)))
        nil
        (let ((vl (car (define-chunks-fct `((isa visual-location
                                              color ,(color obj)
                                              screen-x ,(mine-x (pygame-string *inst*))
                                              screen-y ,(mine-y (pygame-string *inst*))
                                              height ,(height obj)
                                              width ,(width obj)
                                              kind ,(kind obj)
                                              value ,(kind obj)))))))

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

            vl)))

(defmethod build-vis-locs-for ((obj shell) vis-mod)
    (if (string= "n" (shell-exist (pygame-string *inst*)))
        nil
        (let ((vl (car (define-chunks-fct `((isa visual-location
                                              color ,(color obj)
                                              screen-x ,(shell-x (pygame-string *inst*))
                                              screen-y ,(shell-y (pygame-string *inst*))
                                              height ,(height obj)
                                              width ,(width obj)
                                              kind ,(kind obj)
                                              value ,(kind obj)))))))

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

            vl)))         

(defmethod build-vis-locs-for ((obj bonus) vis-mod)
    (if (string= "n" (mine-exist (pygame-string *inst*)))
        nil
        (let ((vl (car (define-chunks-fct `((isa visual-location
                                              color ,(color obj)
                                              screen-x ,(x obj)
                                              screen-y ,(y obj)
                                              height ,(height obj)
                                              width ,(width obj)
                                              kind ,(kind obj)
                                              value ,(bonus-symbol (pygame-string *inst*))))))))

            (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 pygame-screen) key)
  (case key
    (#\d (setf *pressedkey* "d"))
    (#\a (setf *pressedkey* "a"))
    (#\Space (setf *pressedkey* "space"))))
    
    
(defmethod get-mouse-coordinates ((device pygame-screen))
    (vector 0 0))

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

(defun visicon-length ()
  (length (visicon-chunks (get-module :vision))))

(defmethod device-update ((self pygame-screen) time)
  (progn 
    (dbus-bus-pump *bus*)
    (proc-display)))
    ;;;;(print "refresh screen!")))
    
(defun desired-angle-by-position (x y)
    "calculates angle ship should reach to enter orbiting circle around fortress using worldsurf coordinates"
    (let ((distance (sqrt (+ (expt (- x 355) 2) (expt (- y 313) 2))))
          (angle-to-tan (asin (/ 140 (sqrt (+ (expt (- x 355) 2) (expt (- y 313) 2))))))
          (angle-from-x (atan (- 313 y) (- 355 x))))
        (if (< distance 140)
            (mod (- 120 (/ (* 3 distance) 14) (* angle-from-x 57.2957795)) 360) ;looking for a reason not to use LISP? Go infix!
            (mod (* 57.2957795 (- angle-to-tan angle-from-x)) 360))))
            
(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 'bonus) (subs *inst*))
    (push (make-instance 'shell) (subs *inst*))
    (push (make-instance 'IFF) (subs *inst*))
    (push (make-instance 'mine) (subs *inst*))
    (push (make-instance 'ship) (subs *inst*))
    (setf (pygame-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

;a ridiculously high visual movement tolerance because it wraps around the screen. Might have to do something about that
;anticipate eye movement?
(sgp :v nil :visual-movement-tolerance 40.0) ;trace-detail high

(chunk-type standard-flight-pattern state shipx shipy shipangle targetangle)
(chunk-type (ship-feature (:include visual-location)))
(chunk-type (shell-feature (:include visual-location)))
(chunk-type (iff-feature (:include visual-location)))
(chunk-type (mine-feature (:include visual-location)))
(chunk-type (bonus-feature (:include visual-location)))
(chunk-type (ship (:include visual-object)))
(chunk-type (shell (:include visual-object)))
(chunk-type (iff (:include visual-object)))
(chunk-type (mine (:include visual-object)))
(chunk-type (bonus (:include visual-object)))
(define-chunks (ship isa chunk))
(define-chunks (shell isa chunk))
(define-chunks (iff isa chunk))
(define-chunks (mine isa chunk))
(define-chunks (bonus isa chunk))

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

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

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

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

(p attend-ship
  =goal>
    ISA          standard-flight-pattern
    state        attend-ship
  =visual-location>
    ISA          visual-location
    kind         ship
  - value        nil
    screen-x     =shipx
    screen-y     =shipy
  ?visual>
    state        free
==>
  +visual>
    ISA          move-attention
    screen-pos   =visual-location
  =goal>
    state        encode-ship
    shipx        =shipx
    shipy        =shipy
)

(p encode-ship
  =goal>
    ISA          standard-flight-pattern
    state        encode-ship
    shipx        =shipx
    shipy        =shipy
  =visual>
    ISA          visual-object
    value        =angle
==>
  !safe-bind! =target (desired-angle-by-position =shipx =shipy)
  =goal>
    shipangle    =angle
    targetangle  =target
    state        adjust-trajectory
)

(p turn-counter-clockwise-normal
  =goal>
    ISA          standard-flight-pattern
    state        adjust-trajectory
    shipangle    =current
    targetangle  =target
    !eval!       (and (> =target =current) (< (- =target =current) 177))
==> 
  !eval! (setf *joyx* -0.3)
  !eval! (setf *joyy* 0)
  =goal>
    state        begin
)

(p turn-clockwise-normal
  =goal>
    ISA          standard-flight-pattern
    state        adjust-trajectory
    shipangle    =current
    targetangle  =target
    !eval!       (and (< =target =current) (< (- =current =target) 177))
==> 
  !eval! (setf *joyx* 0.3)
  !eval! (setf *joyy* 0)
  =goal>
    state        begin
)

(p turn-counter-clockwise-backwards
  =goal>
    ISA          standard-flight-pattern
    state        adjust-trajectory
    shipangle    =current
    targetangle  =target
    !eval!       (and (< =target =current) (> (- =current =target) 177))
==> 
  !eval! (setf *joyx* -0.3)
  !eval! (setf *joyy* 0)
  =goal>
    state        begin
)

(p turn-clockwise-backwards
  =goal>
    ISA          standard-flight-pattern
    state        adjust-trajectory
    shipangle    =current
    targetangle  =target
    !eval!       (and (> =target =current) (> (- =target =current) 177))
==> 
  !eval! (setf *joyx* 0.3)
  !eval! (setf *joyy* 0)
  =goal>
    state        begin
)

(p thrust
  =goal>
    ISA          standard-flight-pattern
    state        adjust-trajectory
    shipangle    =current
    targetangle  =target
    !eval!       (< (abs (- =target =current)) 6)
==>
  !eval! (setf *joyx* 0)
  !eval! (setf *joyy* -0.25)
  =goal>
    state        begin
)

(goal-focus first-goal)
)