/* Howto compile: gcc -o voht voht.c 
 * To use: 
 * gpm -t ps2 -m /dev/psaux -M -t ps2 -m /dev/voht -R msc 
 * repeats on /dev/gpmdata input from both ps2 port and this program 
 * and set XF86Config: protocol type to "MouseSystems" and 
 *     set XF86Config: device to /dev/gpmdata 
 */


#include <sys/types.h>
#include <sys/stat.h>
#include <unistd.h>
#include <fcntl.h>
#include <stdio.h>
#include <math.h>
#include <assert.h>
#include <ctype.h>

/* usually the parameters fall in this range when estpchirp2m worked */
#define LOWER_THRESHOLD -1.5
#define UPPER_THRESHOLD 1.5

void usage()
{
  fprintf(stderr,"Use: Reads 8 projective chirp parameters from stdin\n"
                 "     and moves the mouse accordingly by writing ps2\n"
                 "     protocol data on /dev/voht.  Parameters are\n"
                 "     relative to last frame\n");
}

void calc_motion( double *x, double *y, float *params, 
                  double *delta_x, double *delta_y);
void motion_to_ps2( double delta_x, double delta_y, unsigned char *ps2_out);
//static void ser_read (int fd, unsigned char *bytes, int count);

int main( int argc, char *argv[] )
{
  int fifo;
  int voht_pin_fifo;
  unsigned char ps2_output_bytes[3]; 
  int i=0;
  int exit_flag = 0;
  double x, y, delta_x, delta_y; /* x and y as orbits coords!!*/
  fd_set rfds;

  float pchirp_parameters[8];

  x=0.5, y=0,5;
  if( argc == 2 ) { 
    usage();
    exit(1);
  }

  /* open the fifo for writing */
  fifo = open("/dev/voht", O_WRONLY);
  if( fifo == -1 ) { perror("open"); exit(1); }

  
  /* open the fifo for reading */
  //voht_pin_fifo = open("/dev/voht_pin", O_RDONLY | O_SYNC);
  /* RDWR seems to keep it from looping too much */
  voht_pin_fifo = open("/dev/voht_pin", O_RDWR | O_SYNC);
  if( voht_pin_fifo == -1 ) { perror("open");exit(1); }
  FD_ZERO(&rfds);
  FD_SET(voht_pin_fifo, &rfds);
  
  while(1) { 
     char buffer[1024];
     int j=0;

    /* scan and verify input parameters */
    while( select( voht_pin_fifo+1, &rfds, NULL, NULL, NULL)==1) {
        read( voht_pin_fifo, &(buffer[j++]), 1);
        printf("got %c\n", buffer[j-1]);
        if( !isalnum(buffer[j-1] ) && buffer[j-1] != '1' ) break;
    }

    //if(scanf("%f", &(pchirp_parameters[i]))==0) continue;
    sscanf(buffer, "%f", &(pchirp_parameters[i]));

      printf("%f read\n", pchirp_parameters[i]);
      if( isnan(pchirp_parameters[i]) ) exit_flag = 1;
      if( pchirp_parameters[i] > UPPER_THRESHOLD ||
          pchirp_parameters[i] < LOWER_THRESHOLD ) {
        exit_flag = 1;
        fprintf(stderr, "Crazy big parameters read, ignoring!!\n");
      }

  //  if( exit_flag == 1) exit(1);
    if( exit_flag == 1) continue;

    calc_motion(&x, &y, pchirp_parameters, &delta_x, &delta_y);
    motion_to_ps2(delta_x, delta_y,ps2_output_bytes);

    printf("sending chars!\n");
    fflush(stdout);
    write(fifo, ps2_output_bytes, 3);
  }
}


/* if we knew the resolution of the monitor/screen and how
 * much fov the HMD takes up on it, then we could actually
 * do a calulation here, but for now, do something perty 
 */
/* ps2 goes from -127:1:127 */
#define RANDOM_FUDGE_FACTOR 127

/* based on some motion, calculate some ps2 output */
void motion_to_ps2( double delta_x, double delta_y, unsigned char *ps2_out)
{
   int dx, dy;
 
   dx = delta_x * RANDOM_FUDGE_FACTOR;
   dy = delta_y * RANDOM_FUDGE_FACTOR;
   //assert(dy >= -127 && dy < 127 && dx >= -127 && dx < 127 );
   fprintf(stderr, "Motion dx = %d, dy = %d\n", dx, dy );

   /* flip x/y orbits -> x/y ps2 motion convention */
   ps2_out[0] = (char)0x00;
   ps2_out[1] = (char)dy;
   ps2_out[2] = (char)dx;
}

/* calculate the new position of the coordinates by
   applying the projective coordinate transformation with
   the parameters as determined by estpchirp2m
   locally, x is up/down, y is left/right
*/
void calc_motion( double *x, double *y, float *params, 
                  double *delta_x, double *delta_y)
{ 
   double last_x, last_y;
   double A_11, A_12, A_21, A_22, b_1, b_2, c_1, c_2, denom;

   last_x = *x;
   last_y = *y;

   A_11 = params[0];
   A_12 = params[1];
   b_1  = params[2];
   A_21 = params[3];
   A_22 = params[4];
   b_2  = params[5];
   c_1  = params[6];
   c_2  = params[7];


   denom = (c_1*last_x + c_2*last_y + 1);

   /* update x and y as in screen coords!! */
   *x = (A_11*last_x + A_12*last_y + b_1) / denom;
   *y = (A_21*last_x + A_22*last_y + b_2) / denom;

   *delta_x = *x - last_x;
   *delta_y = *y - last_y;

/* inverse proj coord trans */
/*
   denom = (last.x*c_1-A_11)*(last.y*c_2-A_22)-A_21*A_12+A_21*c_2*last.x+
           last.y*c_1*A_12-last.y*c_1*c_2*last.x;
   pos->y = A_21*b_1 - A_21*last.x - (last.y*c_1)*(b_1 - last.x) +
           (last.x*c_1-A_11)* (b_2-last.y);
   pos->y = pos->y/denom;
   pos->x = (A_12*pos->y + b_1 - c_2*pos->y*last.x - last.x)/
            (last.x*c_1 - A_11);
*/
}

/*
static void ser_read (int fd,
                      unsigned char *bytes,
                      int count)
{
  struct timeval tv;
  fd_set rfds;
  int num_read = 0;
  int read_count;

  FD_ZERO (&rfds);
  FD_SET (fd, &rfds);
  tv.tv_sec = 1;
  tv.tv_usec = 0;

  while ((select (fd+1, &rfds, NULL, NULL, &tv) == 1) &&
         (num_read < count)) {
    read_count = read (fd, &bytes [num_read], count - num_read);
    num_read += read_count;

    FD_ZERO (&rfds);
    FD_SET (fd, &rfds);
  }

  for (; num_read < count; num_read++) {
     bytes [num_read] = '\0';
  }
}
*/