/* LICENSE:
  =========================================================================
    CMPack'04 Source Code Release for OPEN-R SDK 1.1.5-r2 for ERS7
    Copyright (C) 2004 Multirobot Lab [Project Head: Manuela Veloso]
    School of Computer Science, Carnegie Mellon University
    All rights reserved.
    ========================================================================*/

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <fcntl.h>
#include <linux/joystick.h>
#include <sys/poll.h>

#include "Joystick.h"

int Joystick::open(const char *dev) {
  fd = ::open(dev, O_RDONLY);

  if(fd < 0) {
    fprintf(stderr,"Error opening joystick device '%s'\n",dev);
    perror("Joystick error");
    return 0;
  }
  
  ioctl(fd, JSIOCGAXES,               &num_axes);
  ioctl(fd, JSIOCGBUTTONS,            &num_buttons);
  ioctl(fd, JSIOCGNAME(MAX_NAME_LEN), name);
  
  return 1;
}

int Joystick::processEvents(int max_wait_time_ms) {
  
  if(fd == -1)
    return 0;

  struct pollfd polls[1];
  int total_num_events = 0;
  int num_events;
  struct js_event jse;

  polls[0].fd = fd;
  polls[0].events = POLLIN | POLLPRI;
  polls[0].revents = 0;

  num_events = poll(polls,sizeof(polls)/sizeof(polls[0]),max_wait_time_ms);
  while(num_events > 0) {
    total_num_events += num_events;
    if(polls[0].revents & (POLLERR | POLLHUP | POLLNVAL)){
      fprintf(stderr,"error returned from poll on joystick\n");
      return -1;
    }

    if(polls[0].revents & (POLLIN | POLLPRI)){
      if(read(fd, &jse, sizeof(jse)) != sizeof(jse)){
        perror("read failed on pollable joystick\n");
        return -1;
      }

      switch(jse.type & ~JS_EVENT_INIT){
        case JS_EVENT_AXIS:
          //printf("axis %d now has value %d\n",jse.number,jse.value);
          axes[jse.number] = jse.value;
          break;
        case JS_EVENT_BUTTON:
          //printf("button %d now has value %d\n",jse.number,jse.value);
          buttons[jse.number] = jse.value;
          break;
      }
    }

    num_events--;
    if(num_events <= 0)
      num_events = poll(polls,sizeof(polls)/sizeof(polls[0]),0);
  }
  
  if(num_events < 0){
    perror("Unable to poll joystick");
    return -1;
  }

  return total_num_events;
}

int Joystick::close()
{
  if(fd >= 0)
    ::close(fd);
  
  return 1;
}