/* The DIVE software is covered by a license. The use of the software */
/* represents acceptance of the terms and conditions in the license.  */
/* ****************************************************************** */
/* Copyright (c) 1994, Swedish Institute of Computer Science          */


/*----------------------------------------------------------------------------
 * File:   boids.c
 * Copyright: UCL
 * Version: boids.c,v 1.24 1996/08/27 14:21:07 olof Exp
 * 
 * This Application creates some boids in a world.
 * Anthony Steed - A.Steed@cs.ucl.ac.uk
 *
 * TODO: Add an actor interface so that the parameters can be
 *       customised through tcl.

 * BUGS: This program assumes nothing about the passage of
 *       real-time. It assumes that each simulation loop completes in
 *       the alarm period
 * 
 ---------------------------------------------------------------------------*/

#include <X11/Xlib.h>
#include <X11/Xutil.h>
#include <sys/time.h>
#include <time.h>
#include <math.h>

#include "dive/dive.h"
#include "dive/lammopt.h"
#include "dive/ai_util.h"

#ifndef DEFAULT_STARTING_WORLD
#define DEFAULT_STARTING_WORLD "tutorial"
#endif

#define DEFAULT_BOID "http://www.cs.ucl.ac.uk/research/vr/Dive/objects/boid.vr"

/*==========================================================================
  External variables
  ==========================================================================*/

/*==========================================================================
  Local variables
  ==========================================================================*/

static char rcsid[] = "boids.c,v 1.24 1996/08/27 14:21:07 olof Exp";
static void print_rcsid() {if (1) printf("%s\n", rcsid); else print_rcsid();}

/* Structure for boid storage */
typedef struct {
  objid_t boid_id;
  point_t boid_pos;
  point_t boid_vel;
  int resting;
} Boid;

Boid *boids;

/* These control the dynamics of the flock. All are fairly
   self-explanatory and all can be set on the command line. (If you
   have the patience) */

static char* boid_file;
static int number_boids;
static float update_frequency;
static float inertia_factor;
static float pull_factor;
static float proximity_distance;
static float proximity_factor;
static float bound_velocity;
static float max_velocity;
int can_perch;
float perch_time;
float perch_variable;
point_t start_min_pt;
point_t start_max_pt;
point_t world_min_pt;
point_t world_max_pt;



/*==========================================================================
  Local Functions
  ==========================================================================*/

/* ------------------------------------------------------------------------
 * vector functions
 *
 * I peeked in geometry.c and found some vector math but these were
 * missing, or only have static versions
 * 
 * ------------------------------------------------------------------------ */

#define vecscale(v,d,r) (r)->x = (v)->x*d; (r)->y = (v)->y*d; (r)->z = (v)->z*d;
#define veclength(v) (sqrt(((v)->x * (v)->x) + ((v)->y * (v)->y) + ((v)->z * (v)->z)))

#define vecclear(v) (v)->x =0.0; (v)->y = 0.0; (v)->z = 0.0;

void vecnorm(point_t *v, point_t *r) {
  float tmp = veclength(v);
  vecscale(v,1/tmp,r);
}

void veclimit(point_t *v, double l, point_t *r) {
  double t = veclength(v);
  if (t < l) {
    if (v==r) {
      return;
    }
    else {
      r->x = v->x;
      r->y = v->y;
      r->z = v->z;
    }
  }
  else {
    vecscale(v,l/t,r);
  }
}

/* ------------------------------------------------------------------------
 * on_timer
 *      does the boid dynamics. All parameters can be customised
 *
 * ------------------------------------------------------------------------ */

static void
on_timer(void *arg)
{
  int i,j;
  /* Move the boids */
  point_t center;
  point_t lcenter;
  point_t inertia;
  point_t tmp;
  point_t orient[3]; 
  int proximate;
  int active=0;

  orient[1].x = 0.0; orient[1].y = 1.0; orient[1].z = 0.0;

  for (i=0; i< number_boids; i++) {
/*     printf("%i is resting %i active %i\n",i,boids[i].resting,active); */
    if (boids[i].resting<=0) {
      vecadd(&center, &boids[i].boid_pos, &center); /* Flock center */
      vecadd(&inertia, &boids[i].boid_vel, &inertia); /* Flock inertia */
      active++;
    }
  }
  
  if (active) {
    vecscale(&center, 1/(active), &center); /* Flock center */
  }
  
  vecscale(&inertia, inertia_factor/update_frequency, &inertia);

  for (i=0; i< number_boids; i++) {

    if (boids[i].resting > 0) {
      boids[i].resting--;
    }
    else {
      /* Flock Pull */
      vecdiff(&center, &boids[i].boid_pos , &tmp);
      vecscale(&tmp, pull_factor/update_frequency, &tmp);
      vecadd(&boids[i].boid_vel, &tmp, &boids[i].boid_vel);

      /* Flock Inertia */
      vecadd(&boids[i].boid_vel, &inertia, &boids[i].boid_vel); 

      /* Neighbour Avoidance */
      vecclear(&lcenter);
      proximate = 0;
      
      for (j=0; j< number_boids; j++) {
	if (i!=j) {
	  vecdiff(&boids[i].boid_pos, &boids[j].boid_pos , &tmp);
	  if (veclength(&tmp) < proximity_distance) {
	    proximate++;
	    vecadd(&lcenter,&tmp,&lcenter);
	  }
	}
      }
      if (proximate) {
	vecscale(&lcenter,proximity_factor/update_frequency,&lcenter);
	vecadd(&boids[i].boid_vel, &lcenter, &boids[i].boid_vel);
      }
      
      /* Max velocity */
      veclimit(&boids[i].boid_vel, max_velocity, &boids[i].boid_vel);

      /* Calc new position */
      vecscale(&boids[i].boid_vel, 1/update_frequency, &tmp)
      vecadd(&boids[i].boid_pos, &tmp, &boids[i].boid_pos);

      /* World limits */
      if (boids[i].boid_pos.x > world_max_pt.x) 
	boids[i].boid_vel.x += -bound_velocity;
      if (boids[i].boid_pos.x < world_min_pt.x) 
	boids[i].boid_vel.x +=  bound_velocity;
      if (boids[i].boid_pos.y > world_max_pt.y) 
	boids[i].boid_vel.y += -bound_velocity;
      if (boids[i].boid_pos.y < world_min_pt.y)   {
	if (can_perch) {
	  boids[i].boid_pos.y = 0.0;
	  boids[i].boid_vel.y = bound_velocity;
	  boids[i].resting    = (int)(perch_time+ 
                    (int)(perch_variable*drand48()))*update_frequency; 
	}
	else {
	  boids[i].boid_vel.y += bound_velocity;
	}
      }
      if (boids[i].boid_pos.z > world_max_pt.z) 
	boids[i].boid_vel.z += -bound_velocity;
      if (boids[i].boid_pos.z < world_min_pt.z) 
	boids[i].boid_vel.z +=  bound_velocity;

      /* Calculate orientation */
      vecnorm(&boids[i].boid_vel, &tmp);
      cross(&tmp, &orient[1], &orient[2]);
      cross(&orient[1], &orient[2], &orient[0]);

      
      distr_abs_transform(&boids[i].boid_id, orient, &boids[i].boid_pos, 
		     WORLD_C, NULL, NULL);
    }
  }
}



/* ------------------------------------------------------------------------
 * interaction_cb
 *
 *
 * ------------------------------------------------------------------------ */

static void
interaction_cb(objid_t *dest_id, interaction_type_t type, objid_t *pid, 
	       objid_t *src_id, point_t *pt, objid_t *viewid,void *arg)
{
  int boid_number = (int) arg;

  /* No source or no destination! */
  if (!src_id || !dest_id)
    return ;

  switch (type) {
  case DIVE_IA_SELECT:
    /* Kill current boid and make a new one  */
    printf("Select one %i\n", boid_number);
    break;
  default:
    break;
  }
}

static void boids_exit(void *arg)
{
  int i;

  /* For all boids */
  for (i=0;i<number_boids;i++) {
    distr_entity_remove(&boids[i].boid_id, NULL);
  }
}


/* ------------------------------------------------------------------------
 * boids_init
 *
 * Initialization function, read the boids file and creates a flock
 *
 * ------------------------------------------------------------------------ */

static void boids_init(objid_t *wid)
{
  int i;
  entity * oneboid;
  entity * newboid;
  
  boids = (Boid *) malloc((sizeof(Boid)) * number_boids);
  
  if (distr_readURL(wid, boid_file, NULL, &boids[0].boid_id) < 0) {
    dive_error("boids_init: Could not read boids data from %s.", boid_file);
    dive_exit(0);
  }
  oneboid = find_entity(&boids[0].boid_id);
  if (!oneboid) {
    dive_error("boids_init: Could not find boid object.");
    dive_exit(0);
  }

  /* Read the boids from a file - XXX do this once and copy the result */
  for (i=1;i<number_boids;i++) {
    newboid = copy_entity(oneboid);
    boids[i].boid_id = newboid->id;
    distr_new_top(&boids[i].boid_id, NULL);
    
    boids[i].boid_pos.x = start_min_pt.x + 
      (( start_max_pt.x - start_min_pt.x) * drand48());
    boids[i].boid_pos.y = start_min_pt.y + 
      (( start_max_pt.y - start_min_pt.y) * drand48());
    boids[i].boid_pos.z = start_min_pt.z + 
      (( start_max_pt.z - start_min_pt.z) * drand48());

    boids[i].boid_vel.x = 0;
    boids[i].boid_vel.y = 0;
    boids[i].boid_vel.z = 0;

    boids[i].resting = 0;

    /* Register <interaction_cb> to be called each time a select interaction
       signal is sent to the boids */
    callback_register_filter(INTERACTION_SIGNAL, interaction_cb, 0,
			     &boids[i].boid_id, NULL, 0x0, (void *)i);
  }

  dive_add_exit_fn(boids_exit, NULL);

}


/* ------------------------------------------------------------------------
 * main
 *
 * Init system, setup boids and run forever.
 *
 * ------------------------------------------------------------------------ */

void
main(int argc, char *argv[])
{
  char *start_world;
  objid_t wid;
  LambOption opts[] = {
    {"boid", REQ_ARG, &boid_file, setstr, DEFAULT_BOID,  NULL},
    {"number", REQ_ARG, &number_boids, setint, "10",  NULL},
    {"frequency", REQ_ARG, &update_frequency, setfloat, "10",  NULL},
    {"inertia_factor", REQ_ARG, &inertia_factor, setfloat, "0.01",  NULL},
    {"pull_factor", REQ_ARG, &pull_factor, setfloat, "2.0",  NULL},
    {"prox_dist", REQ_ARG, &proximity_distance, setfloat, "5.0",  NULL},
    {"prox_factor", REQ_ARG, &proximity_factor, setfloat, "5.0",  NULL},
    {"bound_velocity", REQ_ARG, &bound_velocity, setfloat, "0.2",  NULL},
    {"max_velocity", REQ_ARG, &max_velocity, setfloat, "15",  NULL},
    {"can_perch", REQ_ARG, &can_perch, setint, "1",  NULL},
    {"perch_time", REQ_ARG, &perch_time, setfloat, "5.0",  NULL},
    {"perch_variable", REQ_ARG, &perch_variable, setfloat, "5.0",  NULL},

    /* Lammopt doesn't do points so we do this the painful way. These
       put the birds in the empty field of the tutorial world */
    {"sxmin", REQ_ARG, &start_min_pt.x, setfloat, "-170.0",  NULL},
    {"sxmax", REQ_ARG, &start_max_pt.x, setfloat, "-130.0",  NULL},
    {"symin", REQ_ARG, &start_min_pt.y, setfloat, "0.0",  NULL},
    {"symax", REQ_ARG, &start_max_pt.y, setfloat, "+20.0",  NULL},
    {"szmin", REQ_ARG, &start_min_pt.z, setfloat, "+130.0",  NULL},
    {"szmax", REQ_ARG, &start_max_pt.z, setfloat, "+170.0",  NULL},

    {"wxmin", REQ_ARG, &world_min_pt.x, setfloat, "-300.0",  NULL},
    {"wxmax", REQ_ARG, &world_max_pt.x, setfloat, "+300.0",  NULL},
    {"wymin", REQ_ARG, &world_min_pt.y, setfloat, "0.0",  NULL},
    {"wymax", REQ_ARG, &world_max_pt.y, setfloat, "+100.0",  NULL},
    {"wzmin", REQ_ARG, &world_min_pt.z, setfloat, "0.0",  NULL},
    {"wzmax", REQ_ARG, &world_max_pt.z, setfloat, "+300.0",  NULL},
  };

  /* Parse the command line arguments */
  start_world = std_init(opts, sizeof(opts)/sizeof(*opts), argc, argv);

  /* Connect to a world, the new world will get identifier wid */
  objid_zero(&wid);
  distr_world_connect(start_world, &wid, NULL);

  /* Read and initialize boids: it will appear in the new world */
  boids_init(&wid);

  /* Initialize alarm frequency. */
  alarm_init(1000/update_frequency); 

  /* Register <on_timer> to be called each time the alarm wakeups. */
  alarm_add(on_timer, NULL);

  /* GO! */
  threads_main_loop();
}