;;
;; behave5.al: flock/herd/school
;;

(define *sim-length* 300)
(define *render?* #t)
(define *repulse-force* .005)
(define *attract-force* .1)
(define *target-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 *target* (vec3 0 0 0))
(define *target-dist* 1.5)
(define *target-rot-rate* 1)

(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))
	)
	
    ; 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 (* *repulse-force* (/ 1 (sqr d)) dir))
		    )
	       (set! force (+ force f))
	       ))
	 )

    ; calc attract force to target
    (set! force (+ force (* *target-force*
			    (sqr (distance *target* pos))
			    (normalize (- *target* pos))
			    )))

    ; calc beast move
    (define desired-change (normalize force))
    (define axis (cross dir desired-change))
    (define angle (degrees (acos (dot dir desired-change))))
    (set! angle (clamp angle (- turn-limit) turn-limit))
    (set! dir (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))
    (color (get-value beast 'color))
    (sphere)))

(define (draw-target)
  (separator
    (translate *target*)
    (uscale .1)
    (color 1 1 1)
    (box)))

(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*)
	))

(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))))
    (add-beast b)
    ))

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

(define (update-target t)
  (let* ((rmat (mat4-rotate (* t *target-rot-rate*) z-axis))
	 )

    (set! *target* (transform-point (vec3 *target-dist* 0 0) rmat))
    ))


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

(sim)

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