;;
;; behave6.al: tag
;;

(define *sim-length* 600)
(define *render?* #t)
(define *repulse-force* .005)
(define *attract-force* .1)
(define *chase-force* .1)
(define *num-beasts* 30)
(define *beasts* '())
(define *speed* .05)
(define *turn-limit* 30)
(define *size* .1)
(define *color* (vec3 .5 .5 .5))
(define *touch-dist* .2)

(define (randR low high) 
  (cond ((vec3? low) (vec3 (randR (xcomp low) (xcomp high))
			   (randR (ycomp low) (ycomp high))
			   (randR (zcomp low) (zcomp high))))
	(else (+ low (* (rand) (- high low))))))

(define (get-value p key)
  (list-ref (assq key p) 1))

(define (set-value p key val)
  (set-cdr! (assq key p) (list val)))

(define (update-beast beast)
  (let ((force 0)
	(pos (get-value beast 'p))
	(dir (get-value beast 'd))
	(speed (get-value beast 'speed))
	(turn-limit (get-value beast 'turn-limit))
	)

    ; if not "it" calculate forces
    (if (not (get-value beast 'it))
	(begin
	
	  ; calc repulse force from other beasts
	  (for i 0 (- *num-beasts* 1) 1
	       ; don't compare beast to itself
	       (if (not (eq? beast (list-ref *beasts* i)))
		   (let* ((other-beast (list-ref *beasts* i))
			  (other-pos (get-value other-beast 'p))
			  (d (distance pos other-pos))
			  (dir (normalize (- pos other-pos)))
			  (f (vec3 0 0 0))
			  )
		     (if (get-value other-beast 'it)
			 (set! f (* *repulse-force* (/ 1 (sqr (/ d 4))) dir))
			 (set! f (* *repulse-force* (/ 1 (sqr d)) dir)))
		     (set! force (+ force f))
		     ))
	       )
	  
	  ; calc attract force to center
	  (set! force (+ force (* *attract-force*
				  (sqr (distance (vec3 0 0 0) pos))
				  (normalize (- (vec3 0 0 0) pos))
				  )))
	  )

	; else if beast is "it" calc force
	(begin
	  ; find closest other beast and chase (no tap-backs)
	  (define closest-d 9999)
	  (define closest-i -1)
	  (for i 0 (- *num-beasts* 1) 1
	       (let ((other-beast (list-ref *beasts* i)))
		 ; don't compare beast to itself
		 (if (and (not (eq? beast other-beast))
			  (not (get-value other-beast 'safe)))
		     (let* (
			    (other-pos (get-value other-beast 'p))
			    (d (distance pos other-pos))
			    )
		       (if (< d closest-d)
			   (begin
			     (set! closest-d d)
			     (set! closest-i i)
			     ))
		       ))))

	  ; calc force of attraction to closest other beast
	  (define other-beast (list-ref *beasts* closest-i))
	  (define other-pos (get-value other-beast 'p))
	  (set! force (- other-pos pos))

	  ; increase speed of "it" beast so will catch someone eventually
	  (set-value beast 'speed (* speed 1.02))
	  (set-value beast 'turn-limit (* turn-limit 1.05))

	  ; if "touch", change to "not it" and touchee becomes "it"
	  (if (< closest-d *touch-dist*)
	      (begin
		(set-value beast 'it #f)
		(set-value beast 'speed (+ *speed* 
					   (randR (- (/ *speed* 1.5)) 
						  (/ *speed* 1.5))))
		(set-value other-beast 'it #t)

		(map (lambda (other) (set-value other 'safe #f)) *beasts*)
		(set-value beast 'safe #t)
		))
	  )
	)


    ; calc beast move
    (define desired-change (normalize force))
    (define axis (cross dir desired-change))
    (define angle (degrees (acos (clamp (dot dir desired-change) -1 1))))
    (set! angle (clamp angle (- turn-limit) turn-limit))
    (set! dir (normalize (transform-point dir (mat4-rotate angle axis))))
    (set-value beast 'd dir)
	 
    ; set new beast position
    (define change (* dir speed))
    (set-value beast 'p (+ pos change))
    ))

(define (update-beasts)
  (map update-beast *beasts*))

(define (draw-beast beast)
  (separator
    (translate (get-value beast 'p))
    (uscale (get-value beast 'size))
    (if (get-value beast 'it)
	(color 1 0 0)
	(color (get-value beast 'color)))
    (sphere)))

(define (draw-beasts)
  (map draw-beast *beasts*))

(define (add-beast p)
  (set! *beasts* (cons p *beasts*)))

(define (make-one-beast)
  (list	(list 'p (vec3 0 0 0))
	(list 'd (vec3 0 1 0))
	(list 'speed *speed*)
	(list 'turn-limit *turn-limit*)
	(list 'size *size*)
	(list 'color *color*)
	(list 'it #f)
	(list 'safe #f)
	))

(define (create-beast)
  (let (
	(b (make-one-beast))
	)
    (set-value b 'p (vec3 (randR -1 1) (randR -1 1) 0))
    (set-value b 'd (normalize (vec3 (randR -1 1) (randR -1 1) 0)))
    (set-value b 'speed (+ *speed* (randR (- (/ *speed* 1.5)) 
					  (/ *speed* 1.5))))
    (set-value b 'turn-limit (+ *turn-limit* 
				(randR (- (/ *turn-limit* 4)) 
				       (/ *turn-limit* 4))))
    (set-value b 'size (+ *size* (randR (- (/ *size* 1.5)) (/ *size* 3))))
    (set-value b 'color (+ *color* (randR (- (/ *color* 1.5)) 
					  (/ *color* 1.5))))
    (set-value b 'it #f)
    (set-value b 'safe #f)
    (add-beast b)
    ))

(define (create-beasts)
  (for i 1 *num-beasts* 1
       (create-beast))

  ; make one of them "it"
  (define beast (list-ref *beasts* 
			  (inexact->exact (floor (* (rand) *num-beasts*)))))
  (set-value beast 'it #t)
  )

(define (sim)
  (begin
    (create-beasts)
    (for i 1 *sim-length* 1
	 (begin
	   (world
	     (camera "main" "orthographic" 'from (vec3 0 0 1.75))
	     (draw-beasts)
	     )
	   (if *render?*
	       (render
		'style "vector"
		'format '(320 242)
		'display-type "sgif"
		'display-name (string-append "Frames/h"
					     (number->string i)
					     ".rgb")
		))
	   (update-beasts)
	   ))))

(sim)

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;