/* nbody.c :   This file is partly based on the original file from I_COLLIDE
   library distribution,  but most of the functions are written for our
   vogl graphics library and simulatioin need.                             */

/*----------------------------- Local Includes -----------------------------*/

#include <stdio.h> 
#include <stdlib.h>
#include <vogl.h>
#include <vodevice.h>
#include <time.h>
#include <string.h>
#include <math.h>
#include <quat.h>
#include <collision.h>
#include "polytope.h"
#include "graphics.h"

/*----------------------------- Local Constants -----------------------------*/

#if 0
#define PRINT_COLLISIONS
#endif

/*------------------------------ Local Macros -------------------------------*/

#define ABS(a) (((a) > 0.0) ? (a) : (-a))

/*------------------------------- Local Types -------------------------------*/


/*------------------------ Local Function Prototypes ------------------------*/

int load_sim_poly(char *fname, Status *status, Polytope **poly);
void vogl_draw_scene(Status *status, float size, int wire);
void draw_poly(__col_Polyhedron *p, int wire);
void openwindow();



/*------------------------------ Local Globals ------------------------------*/

Status        status;
Polytope     *Poly;
int           frames=0;
int           num_objects=0;
int           total_collisions=0;
double        cube_bound=20.0;

extern __col_State state;
extern __col_Polyhedron      __col_polyhedronLibrary[1000];

/*---------------------------------Functions-------------------------------- */

    
/*****************************************************************************\
 @ polygon_area()
 -----------------------------------------------------------------------------
 description : Compute the signed area of the 2D projection of a face
 input       : A face, and which two dimensions to use for the area
               computation 
 output      : area of the projection of the face
 notes       :
\*****************************************************************************/

double polygon_area(__col_Face *face, int axis1, int axis2)
{
    double area;
    col_Fnode *feature;
    __col_Vertex *v1, *v2;

    area = 0.0;
    for (feature=face->verts, v1 = feature->f.v;
	 feature;
	 feature = feature->next, v1 = v2)
    {
	v2 = (feature->next) ?
	    feature->next->f.v : face->verts->f.v;
	area += v1->coords[axis1]*v2->coords[axis2] -
	    v1->coords[axis2]*v2->coords[axis1];
    }
    area /= 2.0;

    return area;
} /** End of polygon_area() **/



/*****************************************************************************\
 @ face_area()
 -----------------------------------------------------------------------------
 description : Compute the unsigned area of a face
 input       : 
 output      : 
 notes       :  
\*****************************************************************************/
double face_area(__col_Face *face)
{
    double area, xy_area, yz_area, xz_area;

    /* project the polygon onto the three coordinate planes and calculate its
       area there */
    xy_area = polygon_area(face, 0, 1);
    yz_area = polygon_area(face, 1, 2);
    xz_area = polygon_area(face, 0, 2);

    /* use the projected areas to calculate the area of the face */
    area = sqrt(xy_area * xy_area + yz_area * yz_area + xz_area * xz_area);
    
    return area;
} /** End of face_area **/

/*****************************************************************************\
 @ polytope_volume()
 -----------------------------------------------------------------------------
 description : Compute the volume of a polyhedron
 input       : 
 output      : 
 notes       :
\*****************************************************************************/
double polytope_volume(__col_Polyhedron *poly)
{
    double volume, area;
    col_Fnode *feature;
    
    volume = 0.0;
    for(feature=poly->faces; feature; feature = feature->next)
    {
	area = face_area(feature->f.f);
	volume -= area * feature->f.f->plane[3];
    }
    volume /= 3.0;
    
    return volume;
} /** End of polytope_volume() **/


/*****************************************************************************\
 @ load_sim_poly()
 -----------------------------------------------------------------------------
 description : Open the simulation specification file and do initialization
 input       : A file, status, pointer to poly
 output      : 
 notes       :
\*****************************************************************************/
int load_sim_poly(char *fname, Status *status, Polytope **poly)
{
       FILE *fp;
       char s[80];
       int n;
       int i = 0;
       double initial_orientation[3];
       double axis[3];
       double dlt;

       if(!(fp = fopen(fname, "r")))
       {
	  fprintf(stderr, "Load simulation file failed.\n");
	  return -1;
       }
       printf("inside load_sim1\n");


            do fscanf(fp, "%s", s); while (!feof(fp) && strcmp(s, "start"));
            if (feof(fp)) return -1;
            fscanf(fp, "%d", &n);
            ALLOCATE(*poly, Polytope, n);

            while(1)  {
                fscanf(fp, "%s", s);
                if (s[0] == '*') break;
                col_instance_polytope(s, &((*poly)[i].id));

                fscanf(fp, "%f %f %f %f", &(*poly)[i].density,  &(*poly)[i].trans_tot[X], &(*poly)[i].trans_tot[Y], &(*poly)[i].trans_tot[Z]);

                fscanf(fp, "%f %f %f", &(*poly)[i].trans_vel[X], &(*poly)[i].trans_vel[Y], &(*poly)[i].trans_vel[Z]);

                fscanf(fp, "%f %f %f", &initial_orientation[X], &initial_orientation[Y], &initial_orientation[Z]);

                q_from_euler((*poly)[i].tot_quat, Q_DEG_TO_RAD(initial_orientation[X]), Q_DEG_TO_RAD(initial_orientation[Y]), Q_DEG_TO_RAD(initial_orientation[Z]));

                fscanf(fp, "%f %f %f %f", &axis[X], &axis[Y], &axis[Z], &dlt);
                q_make((*poly)[i].delta_quat, axis[X], axis[Y], axis[Z], dlt);
                i++;
                }
                
                status->num_polytopes = i;
                for(i=0; i<status->num_polytopes; i++) {
                  col_calc_cuboid((*poly)[i].id, (*poly)[i].object_center, &(*poly)[i].max_radius);

                (*poly)[i].old_colliding = (*poly)[i].colliding = -1;
      /*          dumpCones(state.polytopes[i].polytope);    */
                 }
       printf("inside load_sim5, cuboided\n");

                return 1;
}


/*****************************************************************************\
 @ mat_identity()
 -----------------------------------------------------------------------------
 description : 
 input       : 
 output      : 
 notes       :
\*****************************************************************************/
static void mat_identity(col_Mat4 m)
{
    register int i,j;

    for (i = 0; i < 4; i++)
        for(j = 0; j < 4; j++)
            m[j][i] = (i==j) ? 1.0 : 0.0;
} /** End of mat_identity() **/



/*****************************************************************************\
 @ calc_translation()
 -----------------------------------------------------------------------------
 description : 
 input       : 
 output      : 
 notes       :
\*****************************************************************************/
static void calc_translation(col_Mat4 m, float x, float y, float z)
{
    mat_identity(m);
 
    m[0][3] = x;
    m[1][3] = y;
    m[2][3] = z;
} /** End of calc_translation() **/



/*****************************************************************************\
 @ init_collision()
 -----------------------------------------------------------------------------
 description :

  Read in the polytopes from a file.  

       1.  Read in the object.
       2.  Pass the objects to collision library so it can create Voronoi cones.
       3.  Create initial positions for the objects.
       4.  Give initial velocities for the objects.
       5.  Give initial O(n^2) matrix the relationship between objects
           (bounce, stick, etc). 
 
 input       : 
 output      : 
 notes       :
\*****************************************************************************/
void init_collision(Status *status, Polytope **poly, char *objFile, char *fname)
{
  int  i;

  printf("Loading library file: %s ...", objFile);
  fflush(stdout);

  if (col_open(30, objFile, 1.0,
	       &(status->num_lib_polytopes)) != COL_OK)
  {
      fprintf(stderr, "init_collision: couldn't open collision library\n");
      exit(1);
  }
  
  printf("done.\n");
  fflush(stdout);

  printf("load library finish with %d polytopes\n",status->num_lib_polytopes); 
  load_sim_poly(fname, status, poly);
  printf("load sim poly finish with %d polytopes\n",status->num_polytopes); 

  /* different libcollide matrix allocation methods might have different
     effects on the random number generator, so seed the generator again after
    & initializing the library */
  
  if (status->seed > 0)
  {
      srand(status->seed);
  }

  printf("Activating all pairs (slow for COL_SPARSE_MATRIX representation)...");
  fflush(stdout);
  
  col_activate_all();
  col_enable_nbody();
  
  printf("done.\n");
  fflush(stdout);
  
  if (status->use_cuboid)
  {
      printf("Using fixed size cube for bounding box\n");
      fflush(stdout);

      for (i=0; i < status->num_polytopes; i++)
      {
	  col_use_cuboid((*poly)[i].id);
      }
  }
  
} /** End of init_collision() **/


/*****************************************************************************\
 @ usage()
 -----------------------------------------------------------------------------
 description : 
 input       : 
 output      : 
 notes       :
\*****************************************************************************/
void usage()
{
    printf("usage: nbody [-l <polytope library>] [-f #frames] \n");
    printf("             [-n <sim_poly file>] [-s seed] [-b]\n");
    printf("             seed is an integer to seed the random number generator\n");
    printf("             -b specifies using dynamic bounding boxes rather than\n");
    printf("                fixed-sized bounding cubes\n");
    printf("\n");
    exit(1);
} /** End of usage() **/



/*****************************************************************************\
 @ initialize()
 -----------------------------------------------------------------------------
 description : 

  Control initialization.

          1.  Decide which file the polytopes will be read from.
          2.  Give initial signal to the collision library.
          3.  Read in polytopes.
          4.  Update State.

   state        Updates the state of the simulation (whether do n-body and etc.).
   argc/argv    Variables passed from the environment

 input       : 
 output      : 
 notes       :
\*****************************************************************************/
void initialize(int argc, char **argv, Status *status, Polytope **poly)
{
    char         objFile[256];
    char         fname[256];
    extern char  *optarg;
    int          c;
    int          args_found;
    
    status->seed = 0;
    status->use_cuboid = 1;
    strcpy(objFile, DEFAULT_LIBRARY);
    strcpy(fname, DEFAULT_SIM_POLY);
    status->num_frames = DEFAULT_FRAMES;
    

    args_found = 1;
    
    while ((c = getopt(argc, argv, "bBhHl:L:f:F:n:N:v:V:r:R:d:D:s:S:")) != EOF)
	/* Parse the various options. */
	switch (c)
	{
	case 'l':
	case 'L':
	    strcpy(objFile, optarg);
	    args_found += 2;
	    break;
	case 'n':
	case 'N':
	    strcpy(fname, optarg);
	    args_found += 2;
	    break;
	case 'b':
	case 'B':
	    status->use_cuboid = 0;
	    args_found++;
	    break;
	case 'f':
	case 'F':
	    status->num_frames = atoi(optarg);
	    args_found += 2;
	    break;
	case 's':
	case 'S':
	    status->seed = atoi(optarg);
	    args_found += 2;
	    break;
	case 'h':
	case 'H':
	    args_found++;
	default:
	    usage();
	    break;
	}

    if (args_found != argc)
	usage();
    
    if (status->seed > 0)
    {
	srand(status->seed);
	printf("Random number seed: %d\n", status->seed);
	fflush(stdout);
    }

    init_collision(status, poly, objFile, fname);

    /* allocate space for reporting all n^2 possible collisions, there aren't
       likely to be anywhere near this many */
 
    ALLOCATE(status->all_collisions, col_Report,
	     status->num_polytopes * status->num_polytopes);
    
    openwindow();
    

} /** End of initialize() **/



/*****************************************************************************\
 @ compute_transformation()
 -----------------------------------------------------------------------------
 description : Compute a polytope's current matrix transformation from its
               total translation and rotation
 input       : 
 output      : 
 notes       :
\*****************************************************************************/
static void compute_transformation(Polytope *poly)
{
  col_Mat4    xrot, yrot, zrot, trans;
  col_Mat4    trans_to_center, xrot_cent, zy, zyxt2c;
  col_Mat4    rot, rot_cent;
  
  calc_translation(trans_to_center,
		   -poly->object_center[X], -poly->object_center[Y],
		   -poly->object_center[Z]);
  
  calc_translation(trans,
		   poly->trans_tot[X] + poly->object_center[X],
		   poly->trans_tot[Y] + poly->object_center[Y], 
		   poly->trans_tot[Z] + poly->object_center[Z]);
  q_to_row_matrix(rot, poly->tot_quat);
  __col_matMultXform(rot, trans_to_center, rot_cent);
  __col_matMultXform(trans, rot_cent, poly->tot);
} /** End of compute_transformation() **/


/*****************************************************************************\
 @ compute_collision()
 -----------------------------------------------------------------------------
 description : Compute all object transformations and perform collision test.
               Swap translations and rotations of colliding pairs
 input       : 
 output      : 
 notes       :
\*****************************************************************************/
void compute_collision(Status *status, Polytope *poly)
{
  col_Mat4          T;
  double        angle[3];
  int           i, j, obj1, obj2;
  float         temp;
  q_type        quat_temp;
  col_DistReport *all_dist;
  int dist_pair;
  
  /* save previous status */
  for (i=0; i < status->num_polytopes; i++)
  {
      printf("inside compute_collision1, update old collision\n");
      poly[i].old_colliding = poly[i].colliding;
      poly[i].colliding = -1;
  }



  
  /* inform collision library of new object positions */
  for (i= 0; i < status->num_polytopes; ++i)
  {
      compute_transformation(&(poly[i]));
      col_set_transform(poly[i].id, poly[i].tot);
      printf("inside compute_collision2, set new pose\n");
  }
  
  /* perform collision test */
  if (col_test() != COL_OK)               /* The biggie */
  {
      fprintf(stderr, "col_test didn't return COL_OK\n");
      exit(1);
  }
  printf("collision detection finish\n");
 
  
  /* inquire results of collision test */

   ALLOCATE(all_dist, col_DistReport,
             status->num_polytopes * status->num_polytopes);
  dist_pair = col_report_distance_all(all_dist, status->num_polytopes*status->num_polytopes);
     printf("get dist pairs: %d\n", dist_pair);
     for(i=0;i<dist_pair;i++) {
       printf("pair:%d-%d, dist:%f\n", all_dist[i].ids[0], all_dist[i].ids[1],all_dist[i].distance);
     }
  status->num_collisions =
      col_report_collision_all(status->all_collisions,
			       status->num_polytopes*status->num_polytopes);
      printf("inside compute_collision3, report finish\n");
  
  total_collisions += status->num_collisions;
  printf("collision report finish:%d collisions\n",status->num_collisions);


#ifdef PRINT_COLLISIONS
  if (status->num_collisions)
      printf("FRAME %d: %d collisions\n", frames, status->num_collisions);
#endif


  /* exchange velocities of colliding objects, providing they weren't already
     colliding last frame (i.e. don't keep swapping until objects have
     seperated) */
  for (i= 0; i < status->num_collisions; ++i)
  {
      obj1 = status->all_collisions[i].ids[0];
      obj2 = status->all_collisions[i].ids[1];

#ifdef PRINT_COLLISIONS
      printf("ids: %d %d\n", obj1, obj2);
#endif
      
      poly[obj1].colliding = obj2;
      poly[obj2].colliding = obj1;

      if ((poly[obj1].old_colliding != obj2)
	  && (poly[obj2].old_colliding != obj1)) 
      {
	  temp = poly[obj1].trans_vel[X];
	  poly[obj1].trans_vel[X] = poly[obj2].trans_vel[X];
	  poly[obj2].trans_vel[X] = temp;
	  
	  temp = poly[obj1].trans_vel[Y];
	  poly[obj1].trans_vel[Y] = poly[obj2].trans_vel[Y];
	  poly[obj2].trans_vel[Y] = temp;
	  
	  temp = poly[obj1].trans_vel[Z];
	  poly[obj1].trans_vel[Z] = poly[obj2].trans_vel[Z];
	  poly[obj2].trans_vel[Z] = temp;

	  for (j=0; j<3; j++)
	      quat_temp[j] = poly[obj1].delta_quat[j];
	  for (j=0; j<3; j++)
	      poly[obj1].delta_quat[j] = poly[obj2].delta_quat[j];
	  for (j=0; j<3; j++)
	      poly[obj2].delta_quat[j] = quat_temp[j];
      } 
  }

#ifdef PRINT_COLLISIONS
  if (status->num_collisions)
      printf("\n");
#endif
  
} /** End of compute_collision() **/



/*****************************************************************************\
 @ move_polytopes()
 -----------------------------------------------------------------------------
 description : Moves each polytope by amount described in its rotational and
	       translational velocities.  Checks to see if move exceeds
	       bounding box.
 input       : 
 output      : 
 notes       :
\*****************************************************************************/
void move_polytopes(Status *status, Polytope *poly)
{
  int      i;
  float    trans;
  float    center[3];
  
  /*
     accumulate the changes to the translation and rotation -- this has some
     error accumulation over a long period of time, but we don't really care
     for this application
  */
  for (i= 0; i < status->num_polytopes; ++i)
  {
      printf("inside move_polytopes\n");
      q_mult(poly[i].tot_quat, poly[i].tot_quat, poly[i].delta_quat);

      poly[i].trans_tot[X] += poly[i].trans_vel[X];
      poly[i].trans_tot[Y] += poly[i].trans_vel[Y];
      poly[i].trans_tot[Z] += poly[i].trans_vel[Z];

      center[X] = poly[i].object_center[X] + poly[i].trans_tot[X];
      center[Y] = poly[i].object_center[Y] + poly[i].trans_tot[Y];
      center[Z] = poly[i].object_center[Z] + poly[i].trans_tot[Z];

      /*
	 Check for penetration of the walls of the simulation volume, and
         rebound off the walls if necessary.  This is a bit of a hack, since it
	 only keeps the object center within the walls.
      */
      if (center[X] > cube_bound)
	{
	  trans = center[X] - cube_bound;
	  poly[i].trans_tot[X] -= trans;
	  poly[i].trans_vel[X] = -poly[i].trans_vel[X];
	}
      if (center[X] < -cube_bound)
	{
	  trans = center[X] - (-cube_bound);
	  poly[i].trans_tot[X] -= trans;
	  poly[i].trans_vel[X] = -poly[i].trans_vel[X];
	}
      if (center[Y] > cube_bound)
	{
	  trans = center[Y] - cube_bound;
	  poly[i].trans_tot[Y] -= trans;
	  poly[i].trans_vel[Y] = -poly[i].trans_vel[Y];
	}
      if (center[Y] < -cube_bound)
	{
	  trans = center[Y] - (-cube_bound);
	  poly[i].trans_tot[Y] -= trans;
	  poly[i].trans_vel[Y] = -poly[i].trans_vel[Y];
	}
      if (center[Z] > cube_bound)
	{
	  trans = center[Z] - cube_bound;
	  poly[i].trans_tot[Z] -= trans;
	  poly[i].trans_vel[Z] = -poly[i].trans_vel[Z];
	}
      if (center[Z] < -cube_bound)
	{
	  trans = center[Z] - (-cube_bound);
	  poly[i].trans_tot[Z] -= trans;
	  poly[i].trans_vel[Z] = -poly[i].trans_vel[Z];
	}
    }
} /** End of move_polytopes() **/

 

/*****************************************************************************\
 @ main()
 -----------------------------------------------------------------------------
 description : main program
 input       : 
 output      : 
 notes       :
\*****************************************************************************/
int main(int argc, char **argv)
{
    float size = 1.00;
    int wire = 0;
    short val;


    vinit("X11");
   printf("DRAW_SCENE is , wire: %d\n", wire);
   printf("DRAW_SCENE is , size: %f\n", size);


    initialize(argc, argv, &status, &Poly);

    printf("Simulating %d frames...", status.num_frames);
    fflush(stdout);
    polymode(PYM_FILL);
    while (qread(&val) != REDRAW);

    
/*    for (frames=0; frames < status.num_frames; frames++)   */
   while(1)
    {
     printf("ready to draw\n");
     color(BLACK);
     clear();

	move_polytopes(&status, Poly);
   printf("DRAW_SCENE is going, wire: %d\n", wire);
   printf("DRAW_SCENE is going, size: %f\n", size);
	vogl_draw_scene(&status,size,wire);
   printf("DRAW_SCENE is returned\n");
	compute_collision(&status, Poly);
     if (qtest()) {
        (void)qread(&val);

        switch (val) {

         case 'q':      /* Stop the program */
         case 'Q':
         case 27:
           gexit();
           exit(0);

         case 'w':      /* Set wireframe mode */
           wire = 1;
           break;

         case 's':
           wire = 0;    /* Set solid mode */
           break;

         case '>':      /* zoom 5% */
           size *= 1.05;
           break;

         case '<':      /* zoom -5% */
           size *= 0.95;
           break;

         default:
           ;
        }
      }
    }
    printf("done.\n");
    printf("Total collisions: %d\n", total_collisions);
    printf("Average collisions per frame: %.3f\n",
	   (double)total_collisions / (double)status.num_frames);
    return 0;
} /** End of main() **/


/*****************************************************************************\
\*****************************************************************************/