#!/usr/bin/env python
import rospy
from sensor_msgs.msg import Joy
from exomy.msg import RoverCommand
from locomotion_modes import LocomotionMode
import math

# Define locomotion modes
global locomotion_mode
global motors_enabled
global last_start_button_pressed

locomotion_mode = LocomotionMode.ACKERMANN.value
motors_enabled = True
AXIS_DEADZONE = 0.1
last_start_button_pressed = False
last_webgui_time = None
WEBGUI_PRIORITY_TIMEOUT = 2.0

VELOCITY_EXPO = 2.0

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 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 callback(data):

    global locomotion_mode
    global motors_enabled
    global last_start_button_pressed
    global last_webgui_time

    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

    # 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

    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
    stick_length = min(math.sqrt(x*x + y*y), 1.0)
    rover_cmd.vel = int(100 * math.pow(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", Joy, callback, queue_size=1)
    pub = rospy.Publisher('/rover_command', RoverCommand, queue_size=1)

    rospy.spin()