;;;; FIELDS
proto -v -l -c "1"
proto -v -l -c "(sense 1)"
proto -v -l -c "(+ 1 (sense 1))"

;; Space & Time operations
proto -v -l -c "(rep t 0 (+ t (dt)))"
proto -v -l -c "(all (def timer () (rep t 0 (+ t (dt)))) (timer))" -desired-period 0.3
; Note path system: timer is defined in $PROTO/lib/
proto -v -l -c "(where (sense 1) (timer) 0)"
proto -v -l -c "(red (any-hood (nbr (sense 1))))"
proto -v -l -c "(min-hood+ (nbr-range))"

;; Neighborhood chaining
; Simple distance estimate:
proto -v -l -c "(red (rep d (inf) (mux (sense 1) 0 (min-hood (+ (nbr-range) (nbr d))))))"

;;;; MOTION FROM VECTOR FIELDS
; Calculate vectors... then feed them to the mov actuator
proto -sv -l -act-err 0.1 "(mov (tup 2 1))"
proto -sv -l -m -act-err 0.1 -3d "(mov (tup 0 0.5 3))"

;; Branches & actuation
proto -sv -l -m -act-err 0.1 "(mov (if (sense 1) (tup 2 1) (tup 0 0)))"
proto -sv -l -m -act-err 0.1 "(if (sense 1) (mov (tup 2 1)) (tup 0 0))"
proto -sv -l -m -act-err 0.1 "(mux (sense 1) (mov (tup 2 1)) (tup 0 0))"

proto -n 500 -sv -l -m -act-err 0.1 "(mov (mux (< (distance-to (sense 1)) 30) (tup 1 1) (tup 0 0)))"

;; Motion & time
proto -sv -l -m -act-err 0.1 "(mov (* (sin (* 0.1 (timer))) (tup 1 0)))"

;; Random Motion
(def rand-vec ()
  (tup (rnd -1 1) (rnd -1 1) (rnd -1 1)))
proto -sv -l -m -act-err 0.1 "(mov (rand-vec))" -s 0.1 -dist-dim -10 10 -10 10 -T -desired-period 0.1

(def brownian ()
  (norm (/ (rnd 0 1) (sqrt (dt)))
  	(tup (rnd -1 1) (rnd -1 1) (rnd -1 1))))
proto -sv -l -m -act-err 0.1 "(mov (brownian))" -s 0.1 -dist-dim -10 10 -10 10 -T -desired-period 0.1 -ratio 0.1 -throttle
proto -sv -l -m -act-err 0.1 "(mov (brownian))" -s 0.1 -dist-dim -10 10 -10 10 -T -desired-period 1 -ratio 0.1 -throttle
proto -sv -l -m -act-err 0.1 "(mov (brownian))" -s 0.1 -dist-dim -10 10 -10 10 -T -desired-period 3 -ratio 0.1 -throttle

;; Flocking
(def flock (dir)
  (rep v 
   (tup 0 0 0)
   (let ((d (normalize
             (int-hood
               (if (and (> (nbr-range) 0) (< (nbr-range) (* (comm-range) 0.333)))
                   (* -1 (normalize (nbr-vec)))      ; repel nearby mass
                   (if (> (nbr-range) (* (radio-range) 0.666))
                       (* 0.2 (normalize (nbr-vec))) ; attract distant mass
                       (normalize (nbr v))))))))     ; between, align vectors
     (normalize (+ dir d)))))                        ; mix with preferred dir
proto -n 500 -r 15 -sv -l -m -s 0.5 "(mov (flock (tup 0 0)))"
; Adding guidance to attract them to the center:
proto -n 500 -r 15 -sv -l -m -s 0.5 "(mov (flock (* -0.5 (sense 1) (normalize (coord)))))"

;;;; SCALABILITY AND ROBUSTNESS
;; Self-Healing Distance Estimate
(def distance-to (source)
  (1st 
    (rep (tup d v)                      ; d = distance estimate, v = value rising
         (tup (inf) 0)                  ; initial value
      (mux source 
        (tup 0 0)                       ; source is always distance zero
        (mux (max-hood+                 ; test whether to apply constraint
               (<= (+ (nbr d) (nbr-range) (* v (+ (nbr-lag) (dt)))) d))
          (tup (min-hood+ (+ (nbr d) (nbr-range))) 0) ; apply triangle inequality
          (let ((v0 (/ (comm-range) (* (dt) 12))))    ; rise rate based on info speed
            (tup (+ d (* v0 (dt))) v0)))))))

proto -n 1000 -s 0.1 -v -l "(red (distance-to (sense 1)))"
; Change the source location: note the self-healing...

;; Disperse
(def disperse ()
  (* (/ 1 (int-hood 1))                                ; normalize for neighborhood size
     (int-hood (* (- (nbr-range) (* 0.9 (comm-range))) ; integrate distance from fixed-point...
                  (normalize (nbr-vec))))))            ; ... times direction to neighbor
proto -sv -l -m -act-err 0.1 "(mov (disperse))" -s 0.1 -dim 100 100 -r 20 -n 100 -w
proto -sv -l -m -act-err 0.1 "(mov (disperse))" -s 0.1 -dim 100 100 -r 20 -n 300 -w
proto -sv -l -m -act-err 0.1 "(mov (disperse))" -s 0.1 -dim 100 100 -r 20 -n 1000 -w
; Draw a group of robots elsewhere: note the self-healing...

; These execute in three dimensions as easily as in two:
proto "(mov (flock (tup 0 0 0)))" -dim 50 50 50 -r 15 -sv -m -n 200
proto "(mov (disperse))" -dim 50 50 50 -r 15 -sv -m -w -n 200


;;;; COMPOSITION
;; Functional composition
(def dilate (source rad)
  (< (distance-to source) rad))
proto -v -l -n 100 -s 0.1 "(red (dilate (sense 1) 30))"
proto -v -l -n 1000 -s 0.1 "(red (dilate (sense 1) 30))"
; Note: scalability carries through composition

(def broadcast (source sent)
  (rep received sent
    (mux source sent
      (2nd (min-hood (nbr (tup (distance-to source) received)))))))
proto -v -l -c -n 200 -s 0.1 "(red (broadcast (sense 1) (sense 2)))"

(def distance (region1 region2)
  (broadcast region2 (distance-to region1)))
proto "(distance (sense 1) (sense 2))" -n 100 -v -r 15 -mag 2 -s 1
; Note: self-healing carries through composition

;; Vector field = gradient of potential field
(def grad (v)
  (* (/ 1 (int-hood 1)) ; normalize over neighborhood
     (int-hood (if (or (= (nbr-range) 0) (not (< (abs (- v (nbr v))) (inf))))
                   (tup 0 0 0) ; ignore singularity
                   (* (/ (- v (nbr v)) (nbr-range))
                      (normalize (nbr-vec)))))))

(def cluster-to (source)
  (grad (distance-to source)))
proto -n 500 -w -m -sv "(mov (mux (sense 1) (tup 0 0 0) (cluster-to (sense 1))))" -s 0.1

(def contour-field (field level c)
  (let* ((vec (grad field)))
    (+ (* c (- level field) vec)
       (rotate (* -0.5 pi) vec))))
(def swirl (src d v)
  (let ((cf (contour-field (distance-to src) d v)))
    (mux (or src (not (< (vlen cf) inf))) (tup 0 0 0) cf)))
proto -n 500 -w -m -sv "(mov (swirl (sense 1) 10 0.01))" -s 0.1


;;;; HETEROGENEOUS BEHAVIOR
;; Nav-grad
(def share-distance-to (is-calculating source)
  (let ((base (if is-calculating (distance-to source) (inf))))
    (mux is-calculating base (min-hood (+ (nbr-range) (nbr base))))))

(def nav-grad (is-mover source)
  (let ((g (grad (share-distance-to (not is-mover) source))))
    (mux (and is-mover (> (len g) 0)) (normalize g) (tup 0 0))))

(def random-subset (p) (once (< (rnd 0 1) p)))
proto -n 500 -r 15 -m -l -s 0.1 -sv "(mov (let ((which (random-subset 0.05))) (nav-grad (blue which) (sense 1))))" -led-stacking 2
;; Failure due to neighborhood overloading:
proto -n 500 -r 15 -m -l -s 0.1 -sv "(mov (let ((which (random-subset 0.2))) (nav-grad (blue which) (sense 1))))" -led-stacking 2
;; Mixing dispersion and nav-grad:
proto -n 500 -r 15 -m -l -s 0.1 -sv "(mov (let ((which (random-subset 0.2))) (+ (if which (disperse) (tup 0 0)) (nav-grad (blue which) (sense 1)))))" -led-stacking 2
;; Note the non-uniform density...

;; Guided flocking
(def flock-demo ()
  (mov
    (mux (sense 1)
      (flock (vmul 0.5 (nav-grad (sense 1) (sense 2))))
      (vmul 0.1 (if (not (sense 1)) (disperse) (tup 0 0))))))
proto -n 500 -r 15 -sv -l -m -s 0.5 -w "(flock-demo)"

;; Search and rescue
(def falling-edge (v)
  (muxand (not v) (delay v)))

(def search-and-rescue (rescuer victim base)
  (let* ((rescued (falling-edge victim)))
    (mux rescuers
         (letfed ((searching #t 
                    (if (any-hood 
                          (if searching
                              (nbr rescued)
                              (and (< (nbr-range) 3) (nbr base))))
                        (not searching)
                        searching)))
           (flock (nav-grad rescuers 
                    (if (broadcast rescuers searching) victim base))))
         (* 0.1 (if rescuers (tup 0 0 0) (disperse))))))
(def snr-demo ()
  (mov (search-and-rescue (sense 1) (sense 2) (sense 3))))
;; Scalability:
proto -n 100 -r 15 -sv -l -m -s 0.1 -w "(* 5 (snr-demo))"
proto -n 300 -r 15 -sv -l -m -s 0.5 -w "(* 5 (snr-demo))"
proto -n 1000 -r 15 -sv -l -m -s 0.5 -w "(* 5 (snr-demo))"

;; Self-healing --- drag nodes, change states --- it heals!
proto -n 500 -r 15 -sv -l -m -s 0.5 -w "(* 5 (snr-demo))"

;; Total lines of code for search and rescue: 57