#!/usr/bin/env python
import errno
import os
import select
import struct
import time

import rospy
from sensor_msgs.msg import Joy


DEVICE_PATH = '/dev/input/js0'
AXIS_COUNT = 8
BUTTON_COUNT = 12
PUBLISH_RATE_HZ = 20.0

JS_EVENT_BUTTON = 0x01
JS_EVENT_AXIS = 0x02
JS_EVENT_INIT = 0x80
EVENT_SIZE = struct.calcsize('IhBB')


def open_device():
    while not rospy.is_shutdown():
        try:
            fd = os.open(DEVICE_PATH, os.O_RDONLY | os.O_NONBLOCK)
            rospy.loginfo('Joystick verbunden: %s', DEVICE_PATH)
            return fd
        except OSError as exc:
            if exc.errno != errno.ENOENT:
                rospy.logwarn('Joystick kann nicht geoeffnet werden: %s', exc)
            rospy.sleep(1.0)
    return None


def close_device(fd):
    if fd is None:
        return
    try:
        os.close(fd)
    except OSError:
        pass


if __name__ == '__main__':
    rospy.init_node('f710_joy_node')

    publisher = rospy.Publisher('/joy', Joy, queue_size=5)
    rate = rospy.Rate(PUBLISH_RATE_HZ)

    axes = [0.0] * AXIS_COUNT
    buttons = [0] * BUTTON_COUNT
    joystick_fd = None

    while not rospy.is_shutdown():
        if joystick_fd is None:
            joystick_fd = open_device()
            axes = [0.0] * AXIS_COUNT
            buttons = [0] * BUTTON_COUNT
            if joystick_fd is None:
                break

        try:
            readable, _, _ = select.select([joystick_fd], [], [], 0.0)
            if readable:
                event = os.read(joystick_fd, EVENT_SIZE)
                while event and len(event) == EVENT_SIZE:
                    _, value, event_type, number = struct.unpack('IhBB', event)
                    event_type &= ~JS_EVENT_INIT

                    if event_type == JS_EVENT_AXIS and number < len(axes):
                        axes[number] = max(-1.0, min(1.0, value / 32767.0))
                    elif event_type == JS_EVENT_BUTTON and number < len(buttons):
                        buttons[number] = 1 if value else 0

                    try:
                        event = os.read(joystick_fd, EVENT_SIZE)
                    except OSError as exc:
                        if exc.errno in (errno.EAGAIN, errno.EWOULDBLOCK):
                            break
                        raise

            message = Joy()
            message.header.stamp = rospy.Time.now()
            message.axes = list(axes)
            message.buttons = list(buttons)
            publisher.publish(message)
            rate.sleep()

        except OSError as exc:
            if exc.errno in (errno.ENODEV, errno.EIO, errno.ENXIO, errno.EBADF):
                rospy.logwarn('Joystick getrennt, warte auf Neuverbindung.')
            else:
                rospy.logwarn('Joystick-Lesefehler: %s', exc)
            close_device(joystick_fd)
            joystick_fd = None
            time.sleep(1.0)

    close_device(joystick_fd)