/*
 * collison-1D.C
 *
 * This program shows how to do collision detection between 
 * two circular objects - disks in one dimension.
 *
 *
 * $Id: collision-1D.C,v 1.8 2013/09/23 00:00:11 gl Exp gl $ 
 */
#include <math.h>
#include <sys/time.h>
#include <stdio.h>
#include <string.h>
#include <GL/gl.h>
#include <GL/glu.h>
#include <GL/glut.h>

#define FALSE 0
#define TRUE 1

// Debugging controls 
enum debug_flags {
  d_wall,
  d_disk
};
const int n_debug_flags = 2;
int debug[n_debug_flags] = { 0, 0 };

// Disks and arena
struct Disk {
  float position;
  float velocity;
  float radius;
  float mass;
  float elasticity;
  GLUquadric *quadric;	// For rendering.
  int slices, loops;	// For rendering.
};

struct Arena {
  float left, right;
};

Arena arena = { -5.0, 5.0 };

const int num_disks = 2;
Disk disk[num_disks] = { 
  { -4.0, 1.0, 0.5, 4.0, 1.0, 0, 10, 3 },
  { 4.0, -1.0, 0.25, 1.0, 1.0, 0, 10, 3 }
};


// Rendering info
enum renderMode { wire, solid };
static renderMode renMode = wire; 
static float elapsedTime = 0.0, startTime = 0.0;
static const int milli = 1000;
static int go = FALSE;
  
void 
setRenderMode(renderMode rm)
{
  if (rm == wire) {
    glDisable(GL_LIGHTING);
    glDisable(GL_DEPTH_TEST);
    glDisable(GL_NORMALIZE);
    glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
  } else if (rm == solid) {
    glEnable(GL_LIGHTING);
    glEnable(GL_LIGHT0);
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_NORMALIZE);
    glShadeModel(GL_SMOOTH);
    glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
  }
}


void 
drawArena(Arena *a)
{
  glBegin(GL_LINES);
  glVertex2f(a->left, -1.0);
  glVertex2f(a->left, 1.0);
  glVertex2f(a->right, -1.0);
  glVertex2f(a->right, 1.0);
  glEnd();
}
  
void 
drawDisk(Disk *p, float sx, float sy, float sz)
{
  if (!p->quadric) {
    fprintf(stderr, "drawDisk: no quadric for disk\n");
    exit(1);
  }
    
  glPushMatrix();
  glScalef(sx, sy, sz);
  gluDisk(p->quadric, 0.0, p->radius, p->slices, p->loops);
  glPopMatrix();
}

void 
drawDisks()
{
  for (int i = 0; i < num_disks; i++) {
    glPushMatrix();
    glTranslatef(disk[i].position, 0.0, 0.0); 
    drawDisk(&disk[i], 1.0, 1.0, 1.0);
    glPopMatrix();
  }
}

void 
changeRenderMode()
{
  if (renMode == wire)
    renMode = solid;
  else
    renMode = wire;
  setRenderMode(renMode);
}

// Integrate equations of motion
void 
integrate()
{
  static float t = 0.0, h;

  // Calculate time increment
  h = elapsedTime - t;
  t = elapsedTime;

  // Compute new positions of disks
  for (int i = 0; i < num_disks; i++)
    disk[i].position += h * disk[i].velocity;
}

// Collision detection and reaction
void 
collide()
{
  float distance, sum_radii;
  float m1, m2, M;
  float v1i, v2i, v1f, v2f;
  int i, j;

  // Collisions amongst disks? 
  for (i = 0; i < num_disks - 1; i++) {
    v1i = disk[i].velocity;
    m1 = disk[i].mass;
    for (j = i + 1; j < num_disks; j++) {
      sum_radii = disk[i].radius + disk[j].radius;
      distance = fabs(disk[j].position - disk[i].position);
      if (distance <= sum_radii) {
	v2i = disk[j].velocity;
	m2 = disk[j].mass;
	M = m1 + m2;
	v1f = (m1 - m2) / M * v1i + 2.0 * m2 / M * v2i;
	v2f = 2 * m1 / M * v1i + (m2 - m1) / M * v2i;
	disk[i].velocity = v1f;
	disk[j].velocity = v2f;
      }
    }
  }

  // Collisions against walls?
  for (i = 0; i < num_disks; i++) {
    if (debug[d_wall]) {
      printf("disk: %d %f\n", i, disk[i].position);
    }
    if ((disk[i].position - disk[i].radius) <= arena.left || 
	(disk[i].position + disk[i].radius) >= arena.right) {
      disk[i].velocity *= -1;
      if (debug[d_wall])
	printf("wall bounce: %d\n", i);
    }
  }
}

// Idle callback
void update()
{
  if (!go) 
    return;

  elapsedTime = glutGet(GLUT_ELAPSED_TIME) / (float)milli - startTime;

  integrate();
  collide();

  glutPostRedisplay();
}

void display()
{
  GLenum err;

  glMatrixMode(GL_MODELVIEW);
  glLoadIdentity();

  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

  glColor3f (0.8, 0.8, 0.8);

  // Draw arena and disks
  glPushMatrix();
  drawArena(&arena);
  drawDisks();
  glPopMatrix();

  glutSwapBuffers();

  // Always check for errors
  while ((err = glGetError()) != GL_NO_ERROR)
    printf("%s\n",gluErrorString(err));
}

void myInit () 
{
  setRenderMode(renMode);

  // Allocate quadrics, one per disk so each can have different LOD 
  for (int i = 0; i < num_disks; i++) 
    if (!disk[i].quadric)
      disk[i].quadric = gluNewQuadric();
}

void keyboardCB(unsigned char key, int x, int y)
{
  switch (key) {
  case 'w':
    changeRenderMode();
    break;
  case 's':
    if (!go) {
      startTime = glutGet(GLUT_ELAPSED_TIME) / (float)milli;
      go = TRUE;
    }
    break;
  }
  glutPostRedisplay();
}

void myReshape(int w, int h)
{
  glViewport(0, 0, w, h);
  glMatrixMode(GL_PROJECTION);
  glLoadIdentity();

  glOrtho(-10.0, 10.0, -10.0, 10.0, -10.0, 10.0);
  glMatrixMode(GL_MODELVIEW);
  glLoadIdentity();
}

/*  Main Loop
 *  Open window with initial window size, title bar, 
 *  RGBA display mode, and handle input events.
 */
int main(int argc, char** argv)
{
  glutInit(&argc, argv);
  glutInitDisplayMode( GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH);
  glutInitWindowSize(400, 400);
  glutInitWindowPosition(500, 500);
  glutCreateWindow("Disks in Arena: Collision Detection");
  glutKeyboardFunc(keyboardCB);
  glutReshapeFunc(myReshape);
  glutDisplayFunc(display);
  glutIdleFunc(update);

  myInit();

  glutMainLoop();
}