/*
 *     POWERGLOVE ROUTINES              Version 3.0
 *
 *
 *
 *     Syracuse University Virtual Reality Lab
 *     
 *    
 *     use  file_id = init_glove();           to set up the glove
 *
 *     use  query_glove(file_id, &pglove);    to get the glove data 
 *                                            and pglove.gesture 
 *                                            and pglove.key
 *
 *
 *     If you have any questions, please contact:
 *
 *                            David Barberi
 *                                   dbarberi@sunsite.unc.edu
 *                      
 *
 *      Output format:
 *
 *      Data Time_stamp  gesture key  x y z  thumb one two three  twist  key1 key2 key3 key4 
 *       |
 *       1= GOOD 
 *       0= CORRUPT
*/ 
/*******************************************************************/
/* stream.c:  Get PowerGlove data and display data values	       */
/*								                                   */
/* Originally written at the Syracuse University Virtual Realiat   */
/* Laboratory by Greg Newby, Will Taroli, Bob Zeh, and David	   */
/* Barberi.  Version 1.0 May 1991.				                   */
/*								                                   */
/* Upgraded to include gesture recognition and glove keypad input  */
/* by David Barberi.  Version 2.0 - 2.7 November 1991.		       */
/*								                                   */
/* Added noise reduction code from David Stampe (posted to the 	   */
/* Powerglove mailing list (glove-list@karazm.math.uh.edu) on	   */
/* 11/15/91 by Greg Newby.			                               */
/*								                                   */
/*								                                   */
/* Contact information:					                     	   */
/*    David Barberi:  dbarberi@sugrfx.acs.syr.edu	        	   */
/*    Greg Newby:     gbnewby@alexia.lis.uiuc.edu		           */
/*******************************************************************/


#include "/ip/vr/stream/powerglove.h"

/*  This include is very important.. contains glove structure */
/*  the Port definition, and #defines */


#include <stdio.h>
#include <fcntl.h>
#include <termios.h>
#include <sys/types.h>
#include "sys/callo.h"
#include <string.h>
#include <sys/time.h>

char char_x[10], char_y[10], char_z[10];
char char_gesture[12], char_key[12];
char char_thumb[5], char_one[5], char_two[5], char_three[5], char_twist[5];
char char_key1[5], char_key2[5], char_key3[5], char_key4[5];




/*******************************************************************/
/* Routine to initialize the serial port                           */
/*******************************************************************/
int init_glove()    /* returns the file_id for the open file */
{
  extern char *sys_errlist[];
  int rslt;
  struct termios tbuf;

  printf(" Welcome to Glove Stream.... \n");

  file_id = open(PORT_ID, O_RDWR, 0);
  
  if (file_id == -1)  {
      printf("Error on open call to %s as Glove port, must be read/writable\n", PORT_ID);
      perror(*sys_errlist);
      return(1);
  } else
     printf(" %s is the Glove port\n", PORT_ID);
  
  tbuf.c_iflag = 0;
  tbuf.c_oflag = 0;
  tbuf.c_lflag = 0;
  tbuf.c_cflag = B9600 | CS8 | CLOCAL | CREAD;

  /* new timewait */
  tbuf.c_cc[VMIN] = 0;		/* Return read even if no chars */
  tbuf.c_cc[VTIME] = 0;		/* Return without waiting */

  rslt = ioctl(file_id, TCSETA, &tbuf);
  if (rslt == -1)   {
      fprintf(stderr, "ioctl failed\n");
      perror(*sys_errlist);
      return(1);
  }
  ioctl(file_id, TCFLSH,2); /* flush the port */
  return(file_id);
}


/*******************************************************************
New Time sleep commands 
by Jan,  ih3e@wilbury.cs.Virginia.EDU
*******************************************************************/

void t_sleep(wait)
unsigned int    wait;

{
    static struct timeval timeout;

	if ( wait < 1000 ) {
 	   timeout.tv_sec = 0;
 	   timeout.tv_usec = wait * 1000;
 	}
	else {
 	   /* integral seconds	*/
       timeout.tv_sec = wait / 1000;
       /* get millisecond part and * 1000 for usecs    */
	   timeout.tv_usec = (wait % 1000) * 1000;
    }

    select(0, 0, 0, 0, &timeout);

}	/* t_sleep */


/*******************************************************************/
/* Puts current glove data in the structure                        */
/*******************************************************************/
int query_glove(file_id, pglove)
     pglove_data *pglove; 
     int file_id;
{
  int ret, temp, data;
  char buf[BUFSIZ];
  char command[BUFSIZ];


  /*  0x01 = Constand Upload (default)
      0x02 = Upload only on Command
      0x04 = Send Default Template (reset)
      0x07 = Reset to Defaults
      0x08 = Rotation Filter ON (default)
      0x09 = Rotation Filter OFF
  */
  
  command[0] = 2;
  
  write(file_id,command,1);

  t_sleep(sleepval);

  ret = read(file_id,buf,14);

  /* Check header bytes to see if the data is good, if bad return a 0 */
  if (!((ret > 3) && (buf[0] == (0x81 - 127)) && (buf[1] == (0xDE - 127)) &&
	(buf[2] == (0xDE - 127)))) {
	    return(0); 
	}

  /* Assign X, Y, and Z (need to query 1st bit for sign on Iris) */
  pglove->x = buf[3]; if (buf[3] > 127) pglove->x -= 256;
  pglove->y = buf[4]; if (buf[4] > 127) pglove->y -= 256;
  pglove->z = buf[5]; if (buf[5] > 127) pglove->z -= 256;  

  /* Adjust scale of Z */
  /*  pglove->z *= ZSCALER; */


  /* Assign rotation and (bit-shifted) rotation values */  
  pglove->twist = buf[6];
  temp = buf[7];
  pglove->three = temp & 3;
  temp = temp >> 2;
  pglove->two = temp & 3;
  temp = temp >> 2;
  pglove->one = temp & 3;
  temp = temp >> 2;
  pglove->thumb = temp & 3;

  /* This is data for the Keypad buttons on the glove */
  temp = buf[8];        
  pglove->key1 = temp & 3;
  temp = temp >> 2;
  pglove->key2 = temp & 3;
  temp = temp >> 2;
  pglove->key3 = temp & 3; 
  temp = temp >> 2;
  pglove->key4 = temp & 3;


  /* new Noise reduction */
  deglitch(pglove);
  dehyst(pglove);

  get_modes();
  return(1);
}
  

/*******************************************************************/
/* dehyst:  low noise removal by D. Stampe		                  */
/*******************************************************************/
#define XHYST 2         /* hysterisis for X, Y low noise reduction */
#define YHYST 2         /* 2 eliminates +/-3 quanta of noise 	   */

static int ox = -1000;  /* last x,y for hysterisis		   */
static int oy = -1000;

dehyst(pglove)    
    pglove_data *pglove;
{
 int x = pglove->x;
 int y = pglove->y;

 if( (pglove->key1==0) && (pglove->key2==0) && (pglove->key3==0) && (pglove->key4==0))
   ox = oy = 0; /* handle recentering ("0"key or "Center") */
    if(x-ox>XHYST) ox = x-XHYST;  /* X hysterisis */
     if(ox-x>XHYST) ox = x+XHYST;
     if(y-oy>YHYST) oy = y-YHYST; /* Y hysterisis */
    if(oy-y>YHYST) oy = y+YHYST;
   pglove->x = ox; /* replace present X,Y data */
  pglove->y = oy;
}


/*******************************************************************/
/* deglitch:  Identify and discard noisy data by D. Stampe         */
/*******************************************************************/
#define XACC 8          /* X, Y maximum accel/decel level. Should  */
#define YACC 8          /* be 6-10, but too high limits gesturing  */

#define XXTEND 2        /* stretches deglitching time 		   */
#define YXTEND 1

static int x1 = 0;  /* delayed 1 sample (for smoothed velocity test) */
static int y1 = 0;
static int x2 = 0;  /* delayed 2 samples */
static int y2 = 0;
static int lx = 0;  /* last good X,Y speed */
static int ly = 0;
static int lax = 0; /* bad data "stretch" counter */
static int lay = 0;
static int lsx = 0; /* X,Y "hold" values to replace bad data */
static int lsy = 0;
static int lcx = 0; /* last X,Y speed for accel. calc. */
static int lcy = 0;

deglitch(pglove)
    pglove_data *pglove; 
{
 int vx, vy;
 int x = pglove->x;
 int y = pglove->y;

  if( (pglove->key1==0) && (pglove->key2==0) && (pglove->key3==0) && (pglove->key4==0))
  /* reset on recentering ("0" or "Center" key) */
  {
   x1 = x2 = y1 = y2 = 0;
    lx = ly = lax = lay = 0;
   lsx = lsy = lcx = lcy = 0;
  }

 vx = x-((x1+x2)>>1); /* smoothed velocity */
 vy = y-((y1+y2)>>1);

 x2 = x1; /* update last values */
  x1 = pglove->x;
  y2 = y1;
 y1 = pglove->y;

 if (abs(lcx-vx) > XACC) lax = XXTEND; /* check for extreme acceleration */
  if (lax == 0) lx = vx; /* save only good velocity */
   lcx = vx; /* save velocity for next accel. */

 if (abs(lcy-vy) > YACC) lay = YXTEND;   /* same deal for Y accel. */
  if (lay == 0) ly=vy;
   lcy = vy;

   if (lax != 0) /* hold X pos'n if glitch */
   {
    pglove->x = lsx;
    lax--;
   }

   if (lay != 0) /* hold Y pos'n if glitch */
   {
    lay--;
    pglove->y = lsy;
   }

lsx = pglove->x; /* save position for X,Y hold */
lsy = pglove->y;
}



/******************************************************************************/
/**  Defines pglove.gesture and pglove.key for easier programming            **/
/******************************************************************************/
get_modes()
{
   pglove.gesture = STRANGE;
   pglove.key = BAD;

   get_hand_mode();
   get_key_mode();

}

/********************* assign pglove.gesture  *************************/
/***       Gesture Recognition                                      ***/
/***       May have to be modified for each person                  ***/
/**********************************************************************/
get_hand_mode()  
{
  
	if( (pglove.one==0) && (pglove.two==0) && (pglove.three==0) ) {
	  pglove.gesture = OPEN; 
	  return(0);
	}

    if( (pglove.one==3) && (pglove.two==3) && (pglove.three==3) ) {
      pglove.gesture = FIST; 
      return(0);
    }
    /* if( (pglove.one>=2) && (pglove.two>=2) && (pglove.three>=2) ) {
        pglove.gesture = FIST;
        return(0);
       } */

 	if( (pglove.one==0) && (pglove.two>=2) && (pglove.three>=2) ) {
 	  pglove.gesture = POINT;
 	  return(0);
 	}
 	 
	if( (pglove.one==0) && (pglove.two==0) && (pglove.three>=2) ) {
	  pglove.gesture = BIGPOINT;
	  return(0);
	}

    if( (pglove.one==3) && (pglove.two==0) && (pglove.three==3) ) {
	  pglove.gesture = BIRD;
	  return(0);
    }
    /* if( (pglove.one>=2) && (pglove.two==0) && (pglove.three>=2) ) {
	     pglove.gesture = BIRD;
	     return(0);
	   } */

	if( (pglove.thumb >=1) && (pglove.one>=2) && (pglove.two==0) && (pglove.three==0) ) {
	  pglove.gesture = AOK;
	  return(0);
	}

	if( (pglove.one>=2) && (pglove.two==0) && (pglove.three==0) ) {
	  pglove.gesture = ONEDOWN;
	  return(0);
	}

    if( (pglove.one==0) && (pglove.two==3) && (pglove.three==0) ) {
      pglove.gesture = TWODOWN;
      return(0);
    }
  
    if( (pglove.one==0) && (pglove.two==0) && (pglove.three==3) ) {
      pglove.gesture = THREEDOWN;
      return(0);
    }

	if( (pglove.one==3) && (pglove.two==3) && (pglove.three<=1) ) {
	  pglove.gesture = THREEUP;
	  return(0);
	}
}

/**********************  pglove.key  **********************/
get_key_mode()
{

	if( (pglove.key1==3) && (pglove.key2==3) && (pglove.key3==3) && (pglove.key4==3) )
		{ pglove.key = NO_KEY; return(0); }

	if( (pglove.key1==0) && (pglove.key2==0) && (pglove.key2==0) && (pglove.key4==0) )
		{ pglove.key = CENTER;  return(0); }   

    if( (pglove.key1==3) && (pglove.key2==0) && (pglove.key3==0) && (pglove.key4==2) )
		{ pglove.key = SELECT; return(0); }

	if( (pglove.key1==2) && (pglove.key2==0) && (pglove.key3==0) && (pglove.key4==2) )
		{ pglove.key = START; return(0); }

	if( (pglove.key1==0) && (pglove.key2==0) && (pglove.key3==0) && (pglove.key4==2) )
		{ pglove.key = ENTER; return(0); }
		
	if( (pglove.key1==1) && (pglove.key2==0) )
		pglove.key = ONE;

	if( (pglove.key1==2) && (pglove.key2==0) )
		pglove.key = TWO;

	if( (pglove.key1==3) && (pglove.key2==0) )
		pglove.key = THREE;

	if( (pglove.key1==0) && (pglove.key2==1) )
		pglove.key = FOUR;

	if( (pglove.key1==1) && (pglove.key2==1) )
		pglove.key = FIVE;

	if( (pglove.key1==2) && (pglove.key2==1) )
		pglove.key = SIX;

	if( (pglove.key1==3) && (pglove.key2==1) )
		pglove.key = SEVEN;

	if( (pglove.key1==0) && (pglove.key2==2) )
		pglove.key = EIGHT;

	if( (pglove.key1==1) && (pglove.key2==2) )
		pglove.key = NINE;

	if( (pglove.key1==1) && (pglove.key2==3) )
		pglove.key = UP;
		/*  for now UP is towards to red LED light */
		
	if( (pglove.key1==2) && (pglove.key2==3) )
		pglove.key = DOWN;

	if( (pglove.key1==3) && (pglove.key2==3) )
		pglove.key = RIGHT;

	if( (pglove.key1==0) && (pglove.key2==3) )
		pglove.key = LEFT;

	if( (pglove.key1==2) && (pglove.key2==2) )
		pglove.key = A;

	if( (pglove.key1==3) && (pglove.key2==2) )
		pglove.key = B;
	
  /*  how to read the data from the keypad  
             key4                  key4
           key3 |                key3 |
         key2 | |              key2 | |
       key1 | | |            key1 | | |
          | | | |               | | | |
  NO_KEY  3 3 3 3           0   0 0 0 0
  UP      1 3 0 0           1   1 0 0 0
  DOWN    2 3 0 0           2   2 0 0 0
  LEFT    0 3 0 0           3   3 0 0 0
  RIGHT   3 3 0 0           4   0 1 0 0
  A       2 2 0 0           5   1 1 0 0
  B       3 2 0 0           6   2 1 0 0 
  SELECT  3 0 0 2           7   3 1 0 0
  START   2 0 0 2           8   0 2 0 0
  CENTER  0 0 0 0           9   1 2 0 0
  PROG    ? ? ? ?       ENTER   0 0 0 2
  ?BAD?   0 0 0 1	                	*/

}



/******************/
print_modes()
{
  
   if(pglove.gesture==OPEN)           sprintf(char_gesture, "OPEN     ");
   if(pglove.gesture==FIST)           sprintf(char_gesture, "FIST     ");
   if(pglove.gesture==POINT)          sprintf(char_gesture, "POINT    ");
   if(pglove.gesture==BIGPOINT)       sprintf(char_gesture, "BIGPOINT ");
   if(pglove.gesture==BIRD)           sprintf(char_gesture, "BIRD     ");
   if(pglove.gesture==AOK)            sprintf(char_gesture, "AOK      ");    
   if(pglove.gesture==ONEDOWN)        sprintf(char_gesture, "ONEDOWN  ");
   if(pglove.gesture==TWODOWN)        sprintf(char_gesture, "TWODOWN  ");
   if(pglove.gesture==THREEDOWN)      sprintf(char_gesture, "THREEDOWN");  
   if(pglove.gesture==STRANGE)        sprintf(char_gesture, "STRANGE  ");

   if(pglove.key==NO_KEY)          sprintf(char_key, "NO_KEY ");
   if(pglove.key==ONE)             sprintf(char_key, "ONE    ");
   if(pglove.key==TWO)             sprintf(char_key, "TWO    ");
   if(pglove.key==THREE)           sprintf(char_key, "THREE  ");
   if(pglove.key==FOUR)            sprintf(char_key, "FOUR   ");
   if(pglove.key==FIVE)            sprintf(char_key, "FIVE   ");
   if(pglove.key==SIX)             sprintf(char_key, "SIX    ");
   if(pglove.key==SEVEN)           sprintf(char_key, "SEVEN  ");
   if(pglove.key==EIGHT)           sprintf(char_key, "EIGHT  ");
   if(pglove.key==NINE)            sprintf(char_key, "NINE   ");
   if(pglove.key==ENTER)           sprintf(char_key, "ENTER  ");
   if(pglove.key==UP)              sprintf(char_key, "UP     ");
   if(pglove.key==DOWN)            sprintf(char_key, "DOWN   ");
   if(pglove.key==RIGHT)           sprintf(char_key, "RIGHT  ");
   if(pglove.key==LEFT)            sprintf(char_key, "LEFT   ");
   if(pglove.key==A)               sprintf(char_key, "A      ");
   if(pglove.key==B)               sprintf(char_key, "B      ");
   if(pglove.key==SELECT)          sprintf(char_key, "SELECT ");
   if(pglove.key==START)           sprintf(char_key, "START  ");
   if(pglove.key==CENTER)          sprintf(char_key, "CENTER ");
   if(pglove.key==BAD)             sprintf(char_key, "BAD    ");

    sprintf(char_x, "%i", pglove.x);
    sprintf(char_y, "%i", pglove.y);
    sprintf(char_z, "%i", pglove.z);
    sprintf(char_thumb, "%i", pglove.thumb);
    sprintf(char_one, "%i", pglove.one);
    sprintf(char_two, "%i", pglove.two);
    sprintf(char_three, "%i", pglove.three);
    sprintf(char_twist, "%i", pglove.twist);
    sprintf(char_key1, "%i", pglove.key1);
    sprintf(char_key2, "%i", pglove.key2);
    sprintf(char_key3, "%i", pglove.key3);
    sprintf(char_key4, "%i", pglove.key4); 
    
}       
