/* ======================================================================= */
/*                                                                         */
/* HandOfDeathPrototype -- 												   */
/*                                                                         */
/*    The wristwatch buzzer activates when the user is "exposed" to the    */
/*    location of the enemy. 								               */
/*                                                                         */
/* ======================================================================= */

#include <windows.h>
#include <GL/gl.h>
#include <GL/glu.h>
#include <GL/glut.h>
#include <stdlib.h>
#include <stdio.h>
#include <termio.h>
#include <fcntl.h>
#include <math.h>


/* =============== Globals ================== */

float px,py,pz,pa,pe,pr;	/* Variables that hold the location and orientation of the polhemus tracker */
int desc_trak, desc_buzz; 	/* file descriptor for port */
static struct termio termconf;
float hand_x, hand_y, hand_z; 


/* Parameters for drawing the graphics */
double win_width  = 800.0;  
double win_height = 600.0;  
double Dv       = -600.00;
double eye_x    =  135.00;
double eye_y    =   30.00;
double eye_z    =  680.00;
double obj_x    =   15.00;
double obj_y    =    0.00;
double obj_z    =  180.00;
double rot_y    =    0.00;
double znear    = .10;
double zfar     = 10000.0;

int show_exposure = 0; /* Flag: Show the plane of exposure or not */


/* ============== Prototypes ================ */
void   send_buzzer_cmd (int, char *);
void   send_fastrak_cmd(int, char *);
void   InitBuzzer      (void);
void   InitFastrak     (void);
void   display         (void);
double mygettime       (void); 
void   myinit          (void);
void   myquit          (void);
/* ========================================== */
int round  (double num) { if ((num - floor(num)) > 0.499) return (floor(num)+1);  else return (floor(num));}
/* ------------------------------------------ */

void draw_square(double x, double y, double z, double h, double v) {
  glBegin(GL_POLYGON); 
    glTexCoord2f(0,1);   glVertex3f(-h/2+x, v/2+y,z);  
    glTexCoord2f(0,0);   glVertex3f(-h/2+x,-v/2+y,z);    
    glTexCoord2f(1,0);   glVertex3f( h/2+x,-v/2+y,z);  
    glTexCoord2f(1,1);   glVertex3f( h/2+x, v/2+y,z); 
  glEnd();
}


void draw_scene (void) {
  /* Draw the scene */
  int x, z, ht = 100, i;


  // walls
  glColor3f(.5,.5,.5);
  glBegin(GL_QUADS);
  for (x = 0; x < 400; x+=100) {
  	/* texture coords are in for future texture mapping */
    glTexCoord2f(0,0);   glVertex3f(x,0,0); 
    glTexCoord2f(0,1);   glVertex3f(x,ht,0); 
    glTexCoord2f(1,1);   glVertex3f(x+100,ht,0); 
    glTexCoord2f(1,0);   glVertex3f(x+100,0,0);   }

    glTexCoord2f(0,0);   glVertex3f(400,0,400); 
    glTexCoord2f(0,1);   glVertex3f(400,ht,400); 
    glTexCoord2f(1,1);   glVertex3f(160,ht,400); 
    glTexCoord2f(1,0);   glVertex3f(160,0,400); 

    glTexCoord2f(0,0);   glVertex3f(100,0,400); 
    glTexCoord2f(0,1);   glVertex3f(100,ht,400); 
    glTexCoord2f(1,1);   glVertex3f(0,ht,400); 
    glTexCoord2f(1,0);   glVertex3f(0,0,400); 


  for (z = 0; z < 400; z+=100) {
    glTexCoord2f(0,0);   glVertex3f(0,0,z); 
    glTexCoord2f(0,1);   glVertex3f(0,ht,z); 
    glTexCoord2f(1,1);   glVertex3f(0,ht,z+100); 
    glTexCoord2f(1,0);   glVertex3f(0,0,z+100); 

    glTexCoord2f(0,0);   glVertex3f(400,0,z); 
    glTexCoord2f(0,1);   glVertex3f(400,ht,z); 
    glTexCoord2f(1,1);   glVertex3f(400,ht,z+100); 
    glTexCoord2f(1,0);   glVertex3f(400,0,z+100); 

    // hall walls
    glTexCoord2f(0,0);   glVertex3f(100,0,z+400); 
    glTexCoord2f(0,1);   glVertex3f(100,ht,z+400); 
    glTexCoord2f(1,1);   glVertex3f(100,ht,z+500); 
    glTexCoord2f(1,0);   glVertex3f(100,0,z+500); 

    glTexCoord2f(0,0);   glVertex3f(160,0,z+400); 
    glTexCoord2f(0,1);   glVertex3f(160,ht,z+400); 
    glTexCoord2f(1,1);   glVertex3f(160,ht,z+500); 
    glTexCoord2f(1,0);   glVertex3f(160,0,z+500);   }

    // main floor
    glColor3f(.3,.3,.3);
    glTexCoord2f(0,0);   glVertex3f(0,0,400); 
    glTexCoord2f(0,1);   glVertex3f(0,0,0); 
    glTexCoord2f(1,1);   glVertex3f(400,0,0);  
    glTexCoord2f(1,0);   glVertex3f(400,0,400); 

    // hall floor
    glTexCoord2f(0,0);   glVertex3f(100,0,400); 
    glTexCoord2f(0,1);   glVertex3f(160,0,400); 
    glTexCoord2f(1,1);   glVertex3f(160,0,800); 
    glTexCoord2f(1,0);   glVertex3f(100,0,800);  


  glEnd();


  // *** ceiling

  glLineWidth(1.0);
  glColor3f(1,1,1);
  for (i = 0; i <= ht; i += ht) {
    glBegin(GL_LINE_LOOP);
      glVertex3f(100,i,800); 
      glVertex3f(100,i,400);
      glVertex3f(0,i,400); 
      glVertex3f(0,i,0); 
      glVertex3f(400,i,0); 
      glVertex3f(400,i,400); 
      glVertex3f(160,i,400); 
      glVertex3f(160,i,800);   
    glEnd();  }

  glBegin(GL_LINES);
    glVertex3f(0,0,0);       glVertex3f(0,ht,0); 
    glVertex3f(0,0,400);     glVertex3f(0,ht,400); 
    glVertex3f(400,0,0);     glVertex3f(400,ht,0); 
    glVertex3f(400,0,400);   glVertex3f(400,ht,400); 
    glVertex3f(100,0,400);   glVertex3f(100,ht,400); 
    glVertex3f(100,0,800);   glVertex3f(100,ht,800); 
    glVertex3f(160,0,400);   glVertex3f(160,ht,400); 
    glVertex3f(160,0,800);   glVertex3f(160,ht,800); 
  glEnd();

  // exposure plane
  if (show_exposure) {
    glColor3f(1,0,0);
    glBegin(GL_QUADS);
      glVertex3f(1,1,1); 
      glVertex3f(1,ht,1); 
      glVertex3f(159,ht,639); 
      glVertex3f(159,0,639);   
    glEnd(); }


  // bad guy
  glColor3f(0,0,1);
  draw_square(5,30,0.1, 10,10);
}


void do_persp (void) {
  /* Do the Perspective transformation */
  glViewport(0, 0, win_width, win_height); 
  glMatrixMode(GL_PROJECTION); 
  glLoadIdentity(); 

  gluPerspective(40.0, win_width/win_height, 1, 3000);
  // glFrustum((-win_width/2),(win_width/2), -win_height,win_height, -Dv, 3000);  

  gluLookAt(eye_x,eye_y,eye_z, obj_x,obj_y,obj_z, 0,1,0); 

  glRotatef(rot_y,0,1,0);
    
  /*  glRotatef(rot_y,0,1,0);
      glTranslatef(obj_x,obj_y,obj_z);  
      glTranslatef(eye_x,eye_y,eye_z);  */

  glMatrixMode(GL_MODELVIEW);
  glLoadIdentity();
}



void display (void) {
  double viewing_d = 500;
  int x,y, tmp, pos;

  glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);  
  glClearColor(0,0,0,0);

  do_persp();

  glRotatef(rot_y, 0,1,0);

  draw_scene();

  /* Draw the location of the user's wrist */
  glColor3f(1,1,1); 

  hand_x = -100-8*px;
  hand_y = 10+10*pz;
  hand_z = 600+-py*6;

  glTranslatef(hand_x, hand_y, hand_z);
  glPointSize(5.0);
  glBegin(GL_POINTS);
    glVertex3f(0,0,0);
  glEnd();
  glColor3f(1,0,1); 
  glutWireCube(2);

  glFlush();
  glutSwapBuffers(); 
}



void InitFastrak() {
  /* Open and set up the interface to the Fastrak on the USB serial device ttyUSB0.
  	 Requires a newer Linux kernal to have support for USB-to-serial adaptor */

  desc_trak = open("/dev/ttyUSB0",O_RDWR /*** | O_NDELAY ***/);

  if ( desc_trak >= 0) printf(". \tPort /dev/ttyUSB0 open.\n"); 
  else            perror("Open port:");
    
  /* Set port characteristics */
  termconf.c_iflag = 0;
  termconf.c_oflag = 0;
  termconf.c_cflag = B9600 | CS8 | CREAD | CLOCAL;
  termconf.c_lflag = ICANON;
  termconf.c_line = 0;
  termconf.c_cc[VTIME] = 0;			
  termconf.c_cc[VMIN] = 0;	        /* packet size */

  if (ioctl(desc_trak, TCSETAW, &termconf) == -1) {
    perror("Tracker-TermSetup");      exit(0);
  }

  send_fastrak_cmd(desc_trak,"H1,0,0,1"); /* Set which hemisphere to use */
  send_fastrak_cmd(desc_trak,"F");        /* Set ascii mode */
  send_fastrak_cmd(desc_trak,"c");        /* Set non-continuous polling */
  printf("\nFastrak Initialized");
}


void send_fastrak_cmd(int desc_trak, char * cmd_buf) {
  /* Send a command to the polhemus device */
  char crlf[3], buf[100];
  
  // printf("Sending: %s\n", cmd_buf);
  sprintf(crlf,"\r\n");
  
  switch ((int) cmd_buf[0]){
  case 'P':
  case 'C':
  case 'c':  
  case 'S':  
      write(desc_trak,cmd_buf,1); 
    break;
  default:
    sprintf(buf,"%s%s",cmd_buf,crlf);
    //strcat(cmd_buf,crlf);
    // cmd_buf = "H1,0,0,1\r\n";

    write(desc_trak, buf, strlen(buf));
  }
}


void InitBuzzer() { 
  /* Set up and initialize the interface to the buzzer device.
     Note that the serial ports are hard coded for this and the polhemus tracker */
  int done = 1;
  desc_buzz = open("/dev/ttyUSB1",O_RDWR /*** | O_NDELAY ***/);

  if ( desc_buzz >= 0) printf(". \tPort /dev/ttyUSB1 open.\n"); 
  else                 perror("Open port:");
    
  termconf.c_iflag = 0;
  termconf.c_oflag = 0;
  termconf.c_cflag = B1200 | CS8 | CREAD | CLOCAL;
  termconf.c_lflag = ICANON;
  termconf.c_line = 0;
  termconf.c_cc[VTIME] = 0;			
  termconf.c_cc[VMIN] = 0;	        /* packet size */

  if (ioctl(desc_buzz, TCSETAW, &termconf) == -1) {
    perror("Buzzer-TermSetup");      exit(0);
  }
  printf("\nBuzzer Initialized");
}


void send_buzzer_cmd(int desc_buzz, char * cmd_buf) { 
  /* Turn the buzzer device on or off*/
  write(desc_buzz,cmd_buf,1); 
}


void myidle(void) {
  /* Idle procedure handles polling the polhemus tracker and looking for exposure */
  char buf[100];
  char oc,er;
  int  st, count, i;

  send_fastrak_cmd(desc_trak,"P");   /* Command to ask for xyzaer data */
  read(desc_trak,buf,47);            /* read data                      */
  
  /* don't change values if not a full record*/
  if( buf[0] == '0' && buf[1] == '1' && buf[45] == '\r') {
    sscanf(buf,"%c%i%c%f%f%f%f%f%f",&oc,&st,&er,&px,&py,&pz,&pa,&pe,&pr); 
    // printf("\n   x: %.2f  y: %.2f  z: %.2f  a: %.2f  e: %.2f  r: %.2f",px,py,pz,pa,pe,pr);   
    glutPostRedisplay();
  }
  else
    printf("\nbad fastrak record: %s",buf);

  /*  I usually get two or three bad records before it gets going...  -jdp */

  //printf("\n hand x,z: %.2f %.2f   %.2f", hand_x, hand_z-400, hand_x/hand_z );


  /* Simplified exposure computation */
  if (0.25*hand_z < hand_x) {
    printf ("\n EXPOSURE.");
    send_buzzer_cmd(desc_buzz, "1"); 
  }
  else send_buzzer_cmd(desc_buzz, "0");

} 


void print_stuff (void) {
  /* print variables (for debugging) */
  printf("\n eye(x,y,z): %7.2f %7.2f %7.2f  obj(x,y,z): %7.2f %7.2f %7.2f", eye_x,eye_y,eye_z, obj_x,obj_y,obj_z);
  printf("\n rot_y: %7.2f\n", rot_y);
}


/* ------------------------------------------ */

void myinit (void) {     
  /* Initialize OpenGL state, polhemus variables */
  glEnable(GL_DEPTH_TEST);
  glDepthFunc(GL_LEQUAL);
  if (use_textures) texture_init();    
  glShadeModel(GL_FLAT);   

  px = 0;  py = 0;  pz = 0;
  pa = 0;  pe = 0;  pr = 0;
}


void myquit (void) { fprintf(stderr, "\n");  exit(0); }

void keyboardFunc (unsigned char key, int x, int y) {
  /* Allows the user to move the viewpoint around the world.
  	 And, to turn on and off the exposure plane. */
  switch (key) {
    case  27:  /*esc*/    myquit();     break;

    case  97:  /* a */   obj_x -= 5;	eye_x -= 5; break;
    case 100:  /* d */   obj_x += 5;	eye_x += 5; break;
    case 115:  /* s */   obj_z += 5;	eye_z += 5; break;
    case 119:  /* w */   obj_z -= 5;	eye_z -= 5; break;

    case 122:  /* z */   obj_y += 10;	eye_y += 10; break;
    case 120:  /* x */   obj_y -= 10;	eye_y -= 10; break;

    case 113:  /* q */   rot_y -= 1;  break;
    case 101:  /* e */   rot_y += 1;  break; 

    case 114:  /* r */   show_exposure=show_exposure?0:1; break;
    case 112:  /* p */   print_stuff(); break;

    case 111:  /* o */   break; 
    case  49:  /* 1 */   break;
  }
}



int main(int argc, char** argv) {
  InitFastrak();
  InitBuzzer();

  glutInitDisplayMode (GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH); 
  glutInitWindowPosition (0, 0);
  glutInitWindowSize(win_width, win_height);
  glutCreateWindow ("Hand Of Death Prototype System");
  // glutFullScreen();      
  glutIdleFunc (myidle);
  glutDisplayFunc (display);
  glutKeyboardFunc(keyboardFunc);

  myinit();

  glutMainLoop();
  myquit();
}