;;Freespace follower

(define-signal center-distance
  (get-center-distance 800))
(define-signal left-distance
  (get-left-distance 600))
(define-signal right-distance
  (get-right-distance 600))

(define-signal stopping-distance 20)
(define-signal orientation-gain 4)
(define-signal distance-gain 40)
(define-signal max-translate 1000)


(define-signal orientation-error
  (temporal-median3 (- left-distance right-distance)))

(define-signal distance-error
  (- center-distance stopping-distance))

(define-signal final-rotate
  (if (< orientation-error 50)
      (clip (* orientation-error orientation-gain 4) -2000 2000)
      (clip (* orientation-error orientation-gain) -2000 2000)))

;;Stuck
(define-signal stuck?  
  (> (true-time (< center-distance 100)) 300))

(define-signal really-stuck?
   (> (true-time (not (is-moving)))  300))
(define-signal unstuck?
  (> (true-time (> center-distance 200)) 300))



;;Stuff  Sensors
(define-signal weapon-near?
  (> (nearest-item 0) -1))
(define-signal ammo-near?
  (> (nearest-item 1) -1))
(define-signal weaponbox-near?
  (> (nearest-item 2) -1))

(define-signal y-weapon
  (yaw-to-item 0 (nearest-item 0)))
(define-signal y-ammo
  (yaw-to-item 1 (nearest-item 1)))
(define-signal y-weaponbox
  (yaw-to-item 2 (nearest-item 2)))

;;Returns the yaw to closest item ranked Weaponbox, Weapon, Ammo
(define-signal y-best-thing
  (cond (weaponbox-near? y-weaponbox)
	(weapon-near?    y-weapon)
	(ammo-near?    y-ammo)))
(define-signal distance-best-thing
  (cond (weaponbox-near? (distance-to-item 0 (nearest-item 0)))
	(weapon-near?    (distance-to-item 1 (nearest-item 1)))
	(ammo-near?      (distance-to-item 2 (nearest-item 2)))))
(define-signal pitch-best-thing
   (cond (weaponbox-near? (pitch-to-item 0 (nearest-item 0)))
	 (weapon-near?    (pitch-to-item 1 (nearest-item 1)))
	 (ammo-near?      (pitch-to-item 2 (nearest-item 2)))))
	
(define-signal stuff-near?
  (and
   (or weapon-near? ammo-near? weaponbox-near?)
   (< distance-best-thing center-distance)
   (< pitch-best-thing 190)))


;;Enemy Sensors
(define-signal enemy-see?
  (> (nearest-visible-enemy) -1))
(define-signal enemy-hear?
   (not (= (distance-to-enemy-sound) 0)))
(define-signal y-enemy
  (yaw-to-player (nearest-visible-enemy)))
(define-signal d-enemy
  (distance-to-player (nearest-visible-enemy)))
(define-signal d
  (+ d-enemy 0))
(define-signal y-enemy-sound
  (yaw-to-enemy-sound))
(define-signal pitch-enemy
  (pitch-to-player (nearest-visible-enemy)))
(define-signal pitch
  (cond ((> pitch-enemy 2) 300)
	((> pitch-enemy 0) 100)
	((< pitch-enemy -2) -300)
	((< pitch-enemy 0) -100)
	(t 0)))

;;Weapons Func
;;Selects weapon based on recently fired (rocket, shotgun)
;;power and ammo
(define-signal select-weapon
  (cond 
   ((and (get-possesses-gun 10)
	 (> (primary-ammo-in-stock 10) 0))
    10)
   ((and (get-possesses-gun 9)
	 (> (primary-ammo-in-stock 9) 0))
    9)
   ((and (get-possesses-gun 8)
	 (> (primary-ammo-in-stock 8) 0))
    8)
   ((and (get-possesses-gun 6)
	 (> (primary-ammo-in-stock 6) 0))
    6)   
   ((and (get-possesses-gun 7)
	 (> (primary-ammo-in-stock 7) 0))
    7)
   ((and (get-possesses-gun 4)
	 (> (primary-ammo-in-stock 4) 0))
     4)
   ((and (get-possesses-gun 3)
	 (> (primary-ammo-in-stock 3) 0))
    3)
   (t 2)))

;;(define-signal select-weapon
;;  (cond
;;   ((get-possesses-gun 10) 10)
;;   ((get-possesses-gun 9) 9)
;;   ((get-possesses-gun 8) 8)
;;   ((get-possesses-gun 7) 7)
;   ((get-possesses-gun 4) 4)
 ;  ((get-possesses-gun 6) 3)
  ; (t 2)))

(define-signal reload?
  (< (ammo-in-clip) 1))
   
(define-signal enough-weapons?
  (or
   (and (> (get-current-weapon) 3)
	(> (primary-ammo-in-stock (get-current-weapon)) 50))
   (and (= (get-current-weapon) 6)
	(> (primary-ammo-in-stock 6) 25))))
  
(define-signal hunt?
  (and enemy-hear? enough-weapons?))

(define-signal health
  (delta (get-health)))

(define-signal loosing-health?
  (not (= health 0)))

(define-signal lost-health-recently?
  (not (> (true-time (not loosing-health?)) 1000)))

(define-signal being-hit
  (and lost-health-recently? (not enemy-see?)))

(define-signal attack-far
  (not (and (> d-enemy 1000)
		(or (= (get-current-weapon) 2)
		    (= (get-current-weapon) 3)))))

(define-signal attack
  (and enemy-see? attack-far))

(define-signal secondary-trigger
  (and (or (= (get-current-weapon) 7)
	   (= (get-current-weapon) 4))
       (< (secondary-ammo-in-stock (get-current-weapon)) 0)))

;;;Behaviors
(define-signal wandering-behavior
  (behavior #t
	    (hl-bot-vector
	     final-rotate
	     max-translate
	     0 0 #f #f #f #f select-weapon
	     #T #f)))

(define-signal stuck-behavior
    (behavior stuck?
	      (latch (if (< right-distance left-distance)
			 (hl-bot-vector 2950 0 0 0 #F #F #F #F 
					select-weapon #T #F)
			 (hl-bot-vector -2950 0 0 0 #F #F #F #F 
					select-weapon #T #F))
		     (not unstuck?))))

(define-signal really-stuck-behavior
  (behavior really-stuck?
	    (latch (if (< right-distance left-distance)
		       (hl-bot-vector 2950 -100 0 0 #F #F #F #F 0 #F #F)
		       (hl-bot-vector -2950 -100 0 0 #F #F #F #F 0 #F #F))
		   (not unstuck?))))

(define-signal weapon-behavior
  (behavior stuff-near?
	    (if (> y-best-thing 0)
		(hl-bot-vector (- (round->integer (* 25 y-best-thing)))
			       max-translate 0 0 #F #F #F #F 0 #F #F)
		(hl-bot-vector (- (round->integer (* 25 y-best-thing)))
			       max-translate 0 0 #F #F #F #F 0 #F #F))))

(define-signal attack-behavior
  (behavior enemy-see?
	    (cond ((> y-enemy 0)
		   (hl-bot-vector (- (round->integer (* 100 y-enemy))) 0 0 pitch #T #F #F 
				  secondary-trigger select-weapon reload? #F))
		  ((< y-enemy 0)
		   (hl-bot-vector (- (round->integer (* 100 y-enemy))) 0 0 pitch #T #F #F
				  secondary-trigger select-weapon reload? #F))
		  (t
		   (hl-bot-vector 0 1 0 pitch #T #F #F secondary-trigger select-weapon reload? #F)))))

(define-signal hunt-behavior
  (behavior hunt?
	    (if (> y-enemy-sound 0)
		(hl-bot-vector (- (round->integer (* 30 y-enemy))) 100 0 0 #F #F #F 
			       secondary-trigger select-weapon reload? #F)
		(hl-bot-vector (- (round->integer (* 30 y-enemy))) 100 0 0 #F #F #F 
			       secondary-trigger select-weapon reload? #F))))

(define-signal being-hit-behavior
  (behavior being-hit
	    (latch (hl-bot-vector 3000 0 0 0 #F #T #F #F select-weapon #F #F)
		   enemy-see?)))

(define-signal go-bot (drive-hl-bot being-hit-behavior 
		       attack-behavior 
		       really-stuck-behavior stuck-behavior
		       weapon-behavior hunt-behavior
		       wandering-behavior))