#!/usr/bin/env python
import ctypes
import fcntl
import glob
import math
import os
import struct
import threading
import time

import rospy
from sensor_msgs.msg import Joy
from exomy.msg import RoverCommand
from locomotion_modes import LocomotionMode

# Define locomotion modes
global locomotion_mode
global motors_enabled
global last_start_button_pressed
global last_locomotion_mode

locomotion_mode = LocomotionMode.ACKERMANN.value
last_locomotion_mode = locomotion_mode
motors_enabled = True
_speed_limit = 100
_speed_limit_last_read = 0.0
AXIS_DEADZONE = 0.1
last_start_button_pressed = False
last_webgui_time = None
WEBGUI_PRIORITY_TIMEOUT = 2.0

VELOCITY_EXPO = 2.0
CARDINAL_SNAP_THRESHOLD = 0.12

DEFAULT_CONTROLLER = "logitech-F710"
CONTROLLER_FUNCTION_MAPS = {
    "webgui": {
        "x_axis": 0,
        "y_axis": 1,
        "invert_x_axis": True,
        "invert_y_axis": False,
        "X_button": 0,
        "Y_button": 3,
        "A_button": 1,
        "B_button": 2,
        "start_button": 9,
        "select_button": 8,
        "sensitivity": 0.15,
    },
    "logitech-F710": {
        "x_axis": 0,
        "y_axis": 1,
        "invert_x_axis": True,
        "invert_y_axis": True,
        "X_button": 0,
        "Y_button": 3,
        "A_button": 1,
        "B_button": 2,
        "start_button": 9,
        "select_button": 8,
        "sensitivity": 0.10,
    },
    "xbox-one": {
        "x_axis": 0,
        "y_axis": 1,
        "invert_x_axis": False,
        "invert_y_axis": False,
        "X_button": 2,
        "Y_button": 3,
        "A_button": 0,
        "B_button": 1,
        "start_button": 7,
        "select_button": 6,
        "sensitivity": 0.11,
    },
}


def get_speed_limit():
    global _speed_limit, _speed_limit_last_read
    now = time.time()
    if now - _speed_limit_last_read > 2.0:
        try:
            _speed_limit = int(rospy.get_param('/speed_limit_percent', 100))
        except Exception:
            pass
        _speed_limit_last_read = now
    return _speed_limit


def _rumble_thread(duration_ms):
    EV_FF = 0x15
    FF_RUMBLE = 0x50
    EVIOCSFF = (0x40000000 | (48 << 16) | (ord('E') << 8) | 0x80)

    device_path = None
    for path in sorted(glob.glob('/dev/input/event*')):
        try:
            sys_name = '/sys/class/input/{0}/device/name'.format(os.path.basename(path))
            with open(sys_name) as f:
                name = f.read().strip()
            if any(k in name for k in ('Gamepad', 'F710', 'GAMEPAD')):
                device_path = path
                break
        except OSError:
            pass

    if not device_path:
        return

    try:
        fd = os.open(device_path, os.O_RDWR)
        try:
            buf = ctypes.create_string_buffer(48)
            struct.pack_into('<H', buf,  0, FF_RUMBLE)
            struct.pack_into('<h', buf,  2, -1)
            struct.pack_into('<H', buf, 10, duration_ms)
            struct.pack_into('<H', buf, 16, 0xFFFF)
            struct.pack_into('<H', buf, 18, 0x8000)
            fcntl.ioctl(fd, EVIOCSFF, buf)
            effect_id = struct.unpack_from('<h', buf, 2)[0]
            import time as _t
            os.write(fd, struct.pack('<QQHHi', 0, 0, EV_FF, effect_id, 1))
            _t.sleep(duration_ms / 1000.0 + 0.05)
            os.write(fd, struct.pack('<QQHHi', 0, 0, EV_FF, effect_id, 0))
        finally:
            os.close(fd)
    except Exception as exc:
        rospy.logwarn('rumble fehler: %s', exc)


def trigger_rumble(duration_ms=150):
    threading.Thread(target=_rumble_thread, args=(duration_ms,)).start()


def apply_deadzone(value, deadzone):
    if abs(value) <= deadzone:
        return 0.0

    scaled = (abs(value) - deadzone) / (1.0 - deadzone)
    return math.copysign(scaled, value)


def get_controller_function_map(data):
    if data.header.frame_id == "webgui":
        return CONTROLLER_FUNCTION_MAPS["webgui"]

    controller_name = rospy.get_param("controller", DEFAULT_CONTROLLER)
    if controller_name in CONTROLLER_FUNCTION_MAPS:
        return CONTROLLER_FUNCTION_MAPS[controller_name]

    rospy.logwarn("Unbekannter Controller '%s', nutze Fallback '%s'.",
                  controller_name, DEFAULT_CONTROLLER)
    return CONTROLLER_FUNCTION_MAPS[DEFAULT_CONTROLLER]


def get_axis_value(axes, axis_index):
    if axis_index < len(axes):
        return axes[axis_index]
    return 0.0


def is_button_pressed(buttons, button_index):
    return button_index < len(buttons) and buttons[button_index] == 1


def snap_axes_to_cardinal(x, y, threshold):
    if abs(x) <= threshold and abs(y) > 0.0:
        return 0.0, math.copysign(abs(y), y)
    if abs(y) <= threshold and abs(x) > 0.0:
        return math.copysign(abs(x), x), 0.0
    return x, y


def callback(data):

    global locomotion_mode
    global motors_enabled
    global last_start_button_pressed
    global last_webgui_time
    global last_locomotion_mode

    is_webgui = data.header.frame_id == "webgui"
    now = rospy.Time.now()

    if is_webgui:
        last_webgui_time = now
    elif last_webgui_time is not None and (now - last_webgui_time).to_sec() < WEBGUI_PRIORITY_TIMEOUT:
        return

    rover_cmd = RoverCommand()

    # Function map for the Logitech F710 joystick
    # Button on pad | function
    # --------------|----------------------
    # A             | Ackermann mode
    # X             | Point turn mode
    # Y             | Crabbing mode
    # Left Stick    | Control speed and direction
    # START Button  | Enable and disable motors

    controller_function_map = get_controller_function_map(data)

    # Reading out joystick data
    y = apply_deadzone(
        get_axis_value(data.axes, controller_function_map["y_axis"]),
        controller_function_map["sensitivity"])
    x = apply_deadzone(
        get_axis_value(data.axes, controller_function_map["x_axis"]),
        controller_function_map["sensitivity"])

    if controller_function_map["invert_x_axis"]:
        x *= -1
    if controller_function_map["invert_y_axis"]:
        y *= -1

    # Kleine Queranteile sollen beim Geradeausfahren nicht zu einem
    # unbeabsichtigten Lenkwinkel fuehren.
    snap_threshold = max(
        controller_function_map["sensitivity"],
        CARDINAL_SNAP_THRESHOLD)
    x, y = snap_axes_to_cardinal(x, y, snap_threshold)

    # Reading out button data to set locomotion mode
    # X Button
    if is_button_pressed(data.buttons, controller_function_map["X_button"]):
        locomotion_mode = LocomotionMode.POINT_TURN.value
    # A Button
    if is_button_pressed(data.buttons, controller_function_map["A_button"]):
        locomotion_mode = LocomotionMode.ACKERMANN.value
    # B Button
    if is_button_pressed(data.buttons, controller_function_map["B_button"]):
        pass
    # Y Button
    if is_button_pressed(data.buttons, controller_function_map["Y_button"]):
        locomotion_mode = LocomotionMode.CRABBING.value

    if not is_webgui and locomotion_mode != last_locomotion_mode:
        trigger_rumble(150)
    last_locomotion_mode = locomotion_mode

    rover_cmd.locomotion_mode = locomotion_mode

    # Enable and disable motors
    # START Button
    start_button_pressed = is_button_pressed(
        data.buttons, controller_function_map["start_button"])
    if start_button_pressed and not last_start_button_pressed:
        if motors_enabled is True:
            motors_enabled = False
            rospy.loginfo("Motors disabled!")
        elif motors_enabled is False:
            motors_enabled = True
            rospy.loginfo("Motors enabled!")
        else:
            rospy.logerr(
                "Exceptional value for [motors_enabled] = {}".format(motors_enabled))
            motors_enabled = False
    last_start_button_pressed = start_button_pressed

    rover_cmd.motors_enabled = motors_enabled

    # The velocity is decoded as value between 0...100, capped by speed limit
    stick_length = min(math.sqrt(x*x + y*y), 1.0)
    rover_cmd.vel = int(100.0 * math.pow(get_speed_limit() / 100.0 * stick_length, VELOCITY_EXPO))

    # The steering is described as an angle between -180...180
    # Which describe the joystick position as follows:
    #   +90
    # 0      +-180
    #   -90
    #
    rover_cmd.steering = int(math.atan2(y, x)*180.0/math.pi)

    rover_cmd.connected = True

    pub.publish(rover_cmd)


if __name__ == '__main__':
    global pub

    rospy.init_node('joystick_parser_node')
    rospy.loginfo('joystick_parser_node started')

    sub = rospy.Subscriber("/joy_delayed", Joy, callback, queue_size=1)
    pub = rospy.Publisher('/rover_command', RoverCommand, queue_size=1)

    rospy.spin()