from math import atan2, degrees, sqrt
from struct import Struct
from time import sleep
from typing import List

import rclpy
from rclpy.context import Context
from rclpy.node import Node
from rclpy.parameter import Parameter
from sensor_msgs.msg import Joy


class JSEvent(object):
    """A joystick even class (struct)

        struct js_event_t
        {
            uint32_t time;
            int16_t value;
            uint8_t type;
            uint8_t id;
        };

    Takes in raw bytes from a file.read(JS_EVENT_SIZE) call.

    Also defines AXIS and BUTTON ids
    """

    JS_EVENT_SIZE = 8  # 64 bits is 8 bytes
    EVENT_BUTTON = 0x01
    EVENT_AXIS = 0x02
    EVENT_INIT = 0x80
    MAX_AXIS_COUNT = 27
    MAX_BUTTON_COUNT = 18
    AXIS_COUNT = 26
    BUTTON_COUNT = 17

    AXIS_LEFT_STICK_HORIZONTAL = 0
    AXIS_LEFT_STICK_VERTICAL = 1
    AXIS_RIGHT_STICK_HORIZONTAL = 2
    AXIS_RIGHT_STICK_VERTICAL = 3
    AXIS_DPAD_UP = 8
    AXIS_DPAD_RIGHT = 9
    AXIS_DPAD_DOWN = 10
    AXIS_DPAD_LEFT = 11  # who knows what the left value should actually be...
    AXIS_LEFT_TRIGGER = 12
    AXIS_RIGHT_TRIGGER = 13
    AXIS_LEFT_BUMPER = 14
    AXIS_RIGHT_BUMPER = 15
    AXIS_TRIANGLE = 16
    AXIS_CIRCLE = 17
    AXIS_X = 18
    AXIS_SQUARE = 19
    AXIS_ACCEL_X = 23  # note: left is positive, right is negative
    AXIS_ACCEL_Y = 24  # note: back is positive, forward is negative
    AXIS_ACCEL_Z = 25  # note: can't tell what sign is what

    BUTTON_SELECT = 0
    BUTTON_LEFT_JOYSTICK = 1
    BUTTON_RIGHT_JOYSTICK = 2
    BUTTON_START = 3
    BUTTON_DPAD_UP = 4
    BUTTON_DPAD_RIGHT = 5
    BUTTON_DPAD_DOWN = 6
    BUTTON_DPAD_LEFT = 7
    BUTTON_LEFT_TRIGGER = 8
    BUTTON_RIGHT_TRIGGER = 9
    BUTTON_LEFT_BUMPER = 10
    BUTTON_RIGHT_BUMPER = 11
    BUTTON_TRIANGLE = 12
    BUTTON_CIRCLE = 13
    BUTTON_X = 14
    BUTTON_SQUARE = 15
    BUTTON_PS3 = 16

    def __init__(self, event_struct):
        # c.f. https://docs.python.org/3/library/struct.html#format-characters
        s = Struct("< I h B B")
        self.time, self.value, self.type, self.id = s.unpack(event_struct)
        # also c.f. Struct.pack()

        # ignore non-input events
        self.type = self.type & ~self.EVENT_INIT

    def __repr__(self):
        struct = "struct js_event_t\n{"
        # struct += '\n\tuint32_t time : {}'.format(self.time)
        struct += "\n\tint16_t value : {}".format(self.value)
        struct += "\n\tuint8_t type  : {}".format(hex(self.type))
        struct += "\n\tuint8_t id    : {}".format(self.id)
        struct += "\n};\n"
        return struct


class Joystick(object):
    def __init__(self, js_file="/dev/input/js1"):
        # note names beginning with `__` are enforced as private
        self.file = open(js_file, "rb")
        # since we're using ID's as indices, account for 0 based indices
        self.__axis_values = [0] * (JSEvent.MAX_AXIS_COUNT + 1)
        # since we're using ID's as indices, account for 0 based indices
        self.__button_values = [0] * (JSEvent.MAX_BUTTON_COUNT + 1)

        self.update_id = -1
        self.AxisUpdate = False
        self.ButtonUpdate = False
        self.event = None

    def __del__(self):
        self.file.close()

    def __enter__(self):
        return self

    # __enter__ and __exit__ allow the context manager syntax
    # with Joystick() as js:
    # ...
    def __exit__(self, exc_type, exc_value, traceback):
        self.file.close()

    def Update(self):
        self.event = JSEvent(self.file.read(JSEvent.JS_EVENT_SIZE))

        if self.event.type == JSEvent.EVENT_AXIS:
            self.update_id = self.event.id
            self.AxisUpdate = True
            self.ButtonUpdate = False
            self.__axis_values[self.event.id] = self.event.value

        if self.event.type == JSEvent.EVENT_BUTTON:
            self.update_id = self.event.id
            self.AxisUpdate = False
            self.ButtonUpdate = True
            self.__button_values[self.event.id] = self.event.value

    def getButtonState(self, button_id):
        return self.__button_values[button_id]

    def getAxisState(self, axis_id):
        return self.__axis_values[axis_id]


class JoystickController(Joystick):
    """An implementation specific controller that inherits a basic Joystick. Adds the capability to
    check for specific updates that you care about and then do something based on that.
    """

    def __init__(self, js_file="/dev/input/js0"):
        Joystick.__init__(self, js_file)
        # direction gives the direction of travel. Defined in terms of r and theta
        self.direction_vector = (0, 0)
        # rotation tells the robot to rotate x degrees
        self.rotation_vector = (0, 0)

    def hasUpdate(self):
        return self.ButtonUpdate or self.AxisUpdate

    def performUpdates(self):
        care_about_buttons = [
            JSEvent.BUTTON_SELECT,
            JSEvent.BUTTON_LEFT_JOYSTICK,
            JSEvent.BUTTON_RIGHT_JOYSTICK,
            JSEvent.BUTTON_START,
            JSEvent.BUTTON_DPAD_UP,
            JSEvent.BUTTON_DPAD_RIGHT,
            JSEvent.BUTTON_DPAD_DOWN,
            JSEvent.BUTTON_DPAD_LEFT,
            JSEvent.BUTTON_LEFT_TRIGGER,
            JSEvent.BUTTON_RIGHT_TRIGGER,
            JSEvent.BUTTON_LEFT_BUMPER,
            JSEvent.BUTTON_RIGHT_BUMPER,
            JSEvent.BUTTON_TRIANGLE,
            JSEvent.BUTTON_CIRCLE,
            JSEvent.BUTTON_X,
            JSEvent.BUTTON_SQUARE,
            JSEvent.BUTTON_PS3,
        ]
        care_about_axes = [
            JSEvent.AXIS_LEFT_STICK_HORIZONTAL,
            JSEvent.AXIS_LEFT_STICK_VERTICAL,
            JSEvent.AXIS_RIGHT_STICK_HORIZONTAL,
            JSEvent.AXIS_RIGHT_STICK_VERTICAL,
            JSEvent.AXIS_DPAD_UP,
            JSEvent.AXIS_DPAD_LEFT,
            JSEvent.AXIS_DPAD_RIGHT,
            JSEvent.AXIS_DPAD_DOWN,
            JSEvent.AXIS_LEFT_TRIGGER,
            JSEvent.AXIS_RIGHT_TRIGGER,
            JSEvent.AXIS_LEFT_BUMPER,
            JSEvent.AXIS_RIGHT_BUMPER,
            JSEvent.AXIS_TRIANGLE,
            JSEvent.AXIS_CIRCLE,
            JSEvent.AXIS_X,
            JSEvent.AXIS_SQUARE,
            JSEvent.AXIS_ACCEL_X,
            JSEvent.AXIS_ACCEL_Y,
            JSEvent.AXIS_ACCEL_Z,
        ]

        if self.ButtonUpdate and self.update_id in care_about_buttons:
            self.__button_update()

        if self.AxisUpdate and self.update_id in care_about_axes:
            self.__axis_update()

    def __button_update(self):
        pass

    def __axis_update(self):
        pass
   
class JoyNode(Node):
    def __init__(
        self,
        node_name: str,
        *,
        context: Context = None,
        cli_args: List[str] = None,
        namespace: str = None,
        use_global_arguments: bool = True,
        enable_rosout: bool = True,
        start_parameter_services: bool = True,
        parameter_overrides: List[Parameter] = None,
        allow_undeclared_parameters: bool = False,
        automatically_declare_parameters_from_overrides: bool = False,
    ) -> None:
        super().__init__(
            node_name,
            context=context,
            cli_args=cli_args,
            namespace=namespace,
            use_global_arguments=use_global_arguments,
            enable_rosout=enable_rosout,
            start_parameter_services=start_parameter_services,
            parameter_overrides=parameter_overrides,
            allow_undeclared_parameters=allow_undeclared_parameters,
            automatically_declare_parameters_from_overrides=automatically_declare_parameters_from_overrides,
        )

        self.pub = self.create_publisher(Joy, "/joy", 1)

    def run(self):
        msg = Joy()
        msg.axes = [0.0, 0.0]
        msg.buttons = [0]
        with JoystickController("/dev/input/js0") as controller:
            while rclpy.ok():
                controller.Update()
                sleep(0.0001)

                if controller.hasUpdate():
                    hor = controller.getAxisState(JSEvent.AXIS_LEFT_STICK_HORIZONTAL)
                    ver = controller.getAxisState(JSEvent.AXIS_LEFT_STICK_VERTICAL)

                    button_change_mode = controller.getButtonState(JSEvent.BUTTON_CIRCLE) # it says circle but it actually is dpad up
                    self.get_logger().info(str(button_change_mode))

                    hor /= 2**15
                    ver /= 2**15

                    msg.axes[JSEvent.AXIS_LEFT_STICK_HORIZONTAL] = hor
                    msg.axes[JSEvent.AXIS_LEFT_STICK_VERTICAL] = ver
                    msg.buttons[0] = button_change_mode

                    self.pub.publish(msg)


def main(args=None):
    rclpy.init(args=args)
    node = JoyNode("test")
    node.run()
    rclpy.shutdown()


if __name__ == "__main__":
    main()