
#include "body_def.h"

void allocate_polys( Polyhedron *p );

// groups cylinder sides in bin tree to minimize collision detection time
Shape *group_cylinder_sides( Polyhedron *p, int section_pos, int section_length );

MultiShape *define_cylinder( double radius, double height,
			     int npts = 12, triple center = 0, int axis = 2 ) {
  if( ( radius <= 0 ) || ( height <= 0 ) || ( npts < 3 ) ) {
    cerr << "define_cylinder -- bad paramaters. Returning NULL\n" << "\tr = " 
	 << radius << "\th = " << height << "\tnpts = " << npts << "\tcom = " 
	 << center << '\n';
    return NULL;
  }
  
  int nvertices = npts * 2;
  int nfaces = npts + 2;
  
  triple offset;
  switch( axis ) {
  case 0:
    offset = triple( height, 0, 0 );
    break;
  case 1:
    offset = triple( 0, height, 0 );
    break;
  case 2:
    offset = triple( 0, 0, height );
    break;
  default:
    cerr << "define_cylinder -- bad axis. returning NULL\n";
    return NULL;
  }

  // for top vertices numbered from 0 to npts-1 ccw
  // for bottom vertices numbered from npts to 2npts-1 cw
  triple *vertices = new triple[ nvertices ];

  // define top at half of height above center
  define_circle( &vertices, center + ( offset / 2 ), radius, npts, axis );
  // shift bottom height below top
  copy_vertices( &vertices[ npts ], vertices, npts, -offset );
  
  // faces 0 to npts - 1 are sides.  npts is top, npts-1 is bottom
  PolyhedronFace *faces = new PolyhedronFace[ nfaces ];
  
  // define side faces
  for( int i = 0 ; i < npts - 1 ; i++ ) {
    faces[i].npts = 4;
    faces[i].indices = new int[ 4 ];
    faces[i].indices[0] = i;
    faces[i].indices[1] = i + npts;
    faces[i].indices[2] = i + npts + 1;
    faces[i].indices[3] = i + 1;
  }
  // define last side piece separately.  indices wrap around
  faces[ npts - 1 ].npts = 4;
  faces[ npts - 1 ].indices = new int[4];
  faces[ npts - 1 ].indices[0] = npts - 1;
  faces[ npts - 1 ].indices[1] = 2 * npts - 1 ;
  faces[ npts - 1 ].indices[2] = npts ;
  faces[ npts - 1 ].indices[3] = 0 ;
  
  // define top face
  faces[npts].npts = npts;
  faces[npts].indices = new int[npts];
  for( int i = 0 ; i < npts ; i++ ) {
    faces[npts].indices[i] = i;
  }
  
  // define bottom face
  faces[ npts + 1 ].npts = npts;
  faces[ npts + 1 ].indices = new int[npts];
  for( int i = 0 ; i < npts ; i++ ) {
    faces[ npts + 1 ].indices[i] = ( 2 * npts - 1 ) - i;
  }

  // define polyhedron
  Polyhedron phdron;// = new Polyhedron;
  ( phdron ).nvertices = nvertices;
  ( phdron ).vertices = vertices;
  ( phdron ).nfaces = nfaces;
  ( phdron ).faces = faces;

  MultiShape *cylinder = new MultiShape;
  // add sides
  cylinder->add( *group_cylinder_sides( &phdron, 0, npts ) );

  // add top and bottom
  cylinder->add( *( ( phdron ).poly3d[npts] ) );
  cylinder->add( *( ( phdron ).poly3d[ npts + 1 ] ) );

  return cylinder;
}

Shape *group_cylinder_sides( Polyhedron *p, int section_pos, 
			     int section_length ) {
  if( section_length == 1 ) {
    return p->poly3d[section_pos];
  } 
  
  MultiShape *m = new MultiShape;
  int hi_half = ( section_length + 1 ) / 2;
  int lo_half = section_length / 2;
  m->add( *group_cylinder_sides( p, section_pos, hi_half ) );
  m->add( *group_cylinder_sides( p, section_pos + hi_half, lo_half ) );
  return m;
}


int body_ind[][4] = { { 0, 1, 2, 3 }, { 0, 6, 5, 1 }, { 0, 3, 7, 6 },
		      { 1, 5, 4, 2 }, { 2, 4, 7, 3 }, { 4, 5, 6, 7 } }; 
MultiShape *define_cube( double xdim, double ydim, double zdim, 
			 triple center = 0 ) {
  
  double half_x = xdim / 2.;
  double half_y = ydim / 2.;
  double half_z = zdim / 2.;
  
  /* allocate space for verices */
  triple *vertex = new triple[8]; 
  
  vertex[0] = triple( -half_x, -half_y, -half_z ) + center;
  vertex[1] = triple( -half_x, half_y, -half_z ) + center;
  vertex[2] = triple( half_x, half_y, -half_z ) + center;
  vertex[3] = triple( half_x, -half_y, -half_z ) + center;
  vertex[4] = triple( half_x, half_y, half_z ) + center;
  vertex[5] = triple( -half_x, half_y, half_z ) + center;
  vertex[6] = triple( -half_x, -half_y, half_z ) + center;
  vertex[7] = triple( half_x, -half_y, half_z ) + center;

  PolyhedronFace *f = new PolyhedronFace[6];
  for( int i = 0; i < 6 ; i++ ) {
    f[i].indices = new int[4];
    f[i].npts = 4;
    for( int j = 0 ; j < 4 ; j++ ) {
      f[i].indices[j] = body_ind[i][j];
    }
  }
  
  Polyhedron phdron; // = new Polyhedron;
  phdron.nvertices = 8;
  phdron.nfaces = 6;
  phdron.vertices = vertex;
  phdron.faces = f;
  
  return def_shape( &phdron, 1 );
}
  
void allocate_polys( Polyhedron &p ) {
  p.poly3d = new Poly3d *[ p.nfaces ];
  
  for( int i = 0 ; i < p.nfaces ; i++ ) {
    p.poly3d[ i ] = new Poly3d;
  }
}

MultiShape *def_shape( Polyhedron *p_array, int nphdrons = 1,
		       boolean allocate_poly3ds = FALSE ) {
  if( nphdrons <= 0 ) {
    cerr << "def_shape -- nphdrons <= 0. returning NULL\n";
    return NULL;
  }
  
  MultiShape *group;
  
  if( nphdrons == 1 ) {
    if( allocate_poly3ds )
      allocate_polys( p_array[0] );
    p_array[0].process();
    group = p_array[0].collect();
  } else {
    group = new MultiShape;
    for( int i = 0; i < nphdrons ; i++ ) {
      group->add( *def_shape( &p_array[i], 1, allocate_poly3ds ) );
    }
  }
  return group;
}

/* fills a vertex list with an n-sided approx of a circle centered at offset.
   allocates space if *vertex empty */
void define_circle( triple **vertex, triple offset = 0, double radius = 1.0, 
		    int num_circ_pts = 12, int axis = 2 ) {
  triple cos_el;
  triple sin_el;
  switch( axis ) {
  case 0:
    cos_el = -y_axis * radius;
    sin_el = -z_axis * radius;
    break;
  case 1:
    cos_el - x_axis * radius;
    sin_el = -z_axis * radius;
    break;
  case 2:
    cos_el = x_axis * radius;
    sin_el = y_axis * radius;
    break;
  default :
    cerr << "define_circle -- bad axis\n";
    return;
  }
  
  if( *vertex == NULL ) {
    *vertex = new triple[ num_circ_pts ];
  }
  
  for( int i = 0 ; i < num_circ_pts ; i++ ) {
    double theta = ( 2 * PI / num_circ_pts ) * i;
    ( *vertex )[i] = offset + cos_el * cos( theta ) + sin_el * sin( theta );
  }
}

/* copies a list of num_points vertices adding offset. */
void copy_vertices( triple *destination, triple *source, int num_points, 
		    triple offset = 0 ) {
  if( source == NULL ) { 
    cerr << "copy_vertices -- null source error\n";
    return;
  }
  
  if( destination == NULL ) {
    cerr << "copy_vertices -- null destination error\n";
    return;
  }
  
  for( int i = 0; i < num_points; i++ ) {
    destination[i] = source[i] + offset;
  }
}

