import wpilib


class PlayStationController:
    CORRECT_FOR_DEADBAND: bool
    DEADBAND: float
    PORT: int
    VERSION: int = 4

    def execute(self) -> None:
        pass

    def setup(self) -> None:
        """
        Setup the PlayStation controller.
        This method is called once when the robot is initialized.
        """
        if self.VERSION == 5:
            self.this_controller = wpilib.PS5Controller(self.PORT)
        else:
            self.this_controller = wpilib.PS4Controller(self.PORT)

        self.button_was_pressed = {
            "Cross": False,
            "Circle": False,
            "Square": False,
            "Triangle": False,
            "DPad_Up": False,
            "DPad_Down": False,
            "DPad_Left": False,
            "DPad_Right": False,
            "L1": False,
            "R1": False,
            "L2": False,
            "R2": False,
            "Options": False,
            "Share": False,
        }
        self.button_is_pressed = {
            "Cross": False,
            "Circle": False,
            "Square": False,
            "Triangle": False,
            "DPad_Up": False,
            "DPad_Down": False,
            "DPad_Left": False,
            "DPad_Right": False,
            "L1": False,
            "R1": False,
            "L2": False,
            "R2": False,
            "Options": False,
            "Share": False,
        }

    def _button_pressed(self, button_name: str, pressed: bool) -> bool:
        """
        Generic method to check if a button is pressed.
        :param button_name: The name of the button to check.
        :param pressed: The current state of the button (True if pressed, False otherwise).
        """
        if pressed:
            self.button_is_pressed[button_name] = True
        else:
            if self.button_is_pressed[button_name]:
                self.button_was_pressed[button_name] = True
            self.button_is_pressed[button_name] = False
        return pressed

    def _button_was_pressed(self, button_name: str) -> bool:
        """
        Generic method to check if a button was pressed since the last check.
        :param button_name: The name of the button to check.
        :return: True if the button was pressed since the last check, False otherwise.
        """
        if self.button_was_pressed[button_name]:
            self.button_was_pressed[button_name] = False
            return True
        return False

    def _corrected_joystick_value(self, raw_value: float) -> float:
        """
        Apply deadband correction to a joystick value.
        :param raw_value: The raw joystick value.
        :return: The corrected joystick value.
        """
        if self.CORRECT_FOR_DEADBAND:
            if abs(raw_value) < self.DEADBAND:
                return 0.0
            return (raw_value / abs(raw_value)) * (
                (abs(raw_value) - self.DEADBAND) / (1 - self.DEADBAND)
            )
        return raw_value

    def capture_button_presses(self) -> None:
        """
        Capture the current state of all buttons to track presses.
        This method should be called at the end of each control loop iteration.
        """
        self.cross_button_pressed()
        self.circle_button_pressed()
        self.square_button_pressed()
        self.triangle_button_pressed()
        self.dpad_up_pressed()
        self.dpad_down_pressed()
        self.dpad_left_pressed()
        self.dpad_right_pressed()
        self.l1_button_pressed()
        self.r1_button_pressed()
        self.l2_button_pressed()
        self.r2_button_pressed()
        self.options_button_pressed()
        self.share_button_pressed()

    def get_joysticks(self) -> tuple[float, float, float, float]:
        """
        Get the joystick values from the Xbox controller.

        :return: A tuple containing the left joystick x, left joystick y, right joystick x, and right joystick y values.
        """
        return (
            self._corrected_joystick_value(self.this_controller.getLeftX()),
            self._corrected_joystick_value(self.this_controller.getLeftY()),
            self._corrected_joystick_value(self.this_controller.getRightX()),
            self._corrected_joystick_value(self.this_controller.getRightY()),
        )

    def cross_button_pressed(self) -> bool:
        """
        Check if the Croiss button is pressed.

        :return: True if the Cross button is pressed, False otherwise.
        """
        return self._button_pressed("Cross", self.this_controller.getCrossButton())

    def cross_button_was_pressed(self) -> bool:
        """
        Check if the Cross button was pressed since the last check.

        :return: True if the Cross button was pressed since the last check, False otherwise.
        """
        return self._button_was_pressed("Cross")

    def circle_button_pressed(self) -> bool:
        """
        Check if the Circle button is pressed.

        :return: True if the Circle button is pressed, False otherwise.
        """
        return self._button_pressed("Circle", self.this_controller.getCircleButton())

    def circle_button_was_pressed(self) -> bool:
        """
        Check if the Circle button was pressed since the last check.

        :return: True if the Circle button was pressed since the last check, False otherwise.
        """
        return self._button_was_pressed("Circle")

    def square_button_pressed(self) -> bool:
        """
        Check if the Square button is pressed.

        :return: True if the Square button is pressed, False otherwise.
        """
        return self._button_pressed("Square", self.this_controller.getSquareButton())

    def square_button_was_pressed(self) -> bool:
        """
        Check if the Square button was pressed since the last check.

        :return: True if the Square button was pressed since the last check, False otherwise.
        """
        return self._button_was_pressed("Square")

    def triangle_button_pressed(self) -> bool:
        """
        Check if the Triangle button is pressed.

        :return: True if the Triangle button is pressed, False otherwise.
        """
        return self._button_pressed(
            "Triangle", self.this_controller.getTriangleButton()
        )

    def triangle_button_was_pressed(self) -> bool:
        """
        Check if the Triangle button was pressed since the last check.

        :return: True if the Triangle button was pressed since the last check, False otherwise.
        """
        return self._button_was_pressed("Triangle")

    def dpad_up_pressed(self) -> bool:
        """
        Check if the D-pad is pressed up.

        :return: True if the D-pad is pressed up, False otherwise.
        """
        return self._button_pressed("DPad_Up", self.this_controller.getPOV() == 0)

    def dpad_up_was_pressed(self) -> bool:
        """
        Check if the D-pad was pressed up since the last check.

        :return: True if the the D-pad was pressed up since the last check, False otherwise.
        """
        return self._button_was_pressed("DPad_Up")

    def dpad_down_pressed(self) -> bool:
        """
        Check if the D-pad is pressed down.

        :return: True if the D-pad is pressed down, False otherwise.
        """
        return self._button_pressed("DPad_Down", self.this_controller.getPOV() == 180)

    def dpad_down_was_pressed(self) -> bool:
        """
        Check if the D-pad was pressed down since the last check.

        :return: True if the the D-pad was pressed down since the last check, False otherwise.
        """
        return self._button_was_pressed("DPad_Down")

    def dpad_left_pressed(self) -> bool:
        """
        Check if the D-pad is pressed left.

        :return: True if the D-pad is pressed left, False otherwise.
        """
        return self._button_pressed("DPad_Left", self.this_controller.getPOV() == 270)

    def dpad_left_was_pressed(self) -> bool:
        """
        Check if the D-pad was pressed left since the last check.

        :return: True if the the D-pad was pressed left since the last check, False otherwise.
        """
        return self._button_was_pressed("DPad_Left")

    def dpad_right_pressed(self) -> bool:
        """
        Check if the D-pad is pressed right.

        :return: True if the D-pad is pressed right, False otherwise.
        """
        return self._button_pressed("DPad_Right", self.this_controller.getPOV() == 90)

    def dpad_right_was_pressed(self) -> bool:
        """
        Check if the D-pad was pressed right since the last check.

        :return: True if the the D-pad was pressed right since the last check, False otherwise.
        """
        return self._button_was_pressed("DPad_Right")

    def r1_button_pressed(self) -> bool:
        """
        Check if the R1 button is pressed.

        :return: True if the R1 button is pressed, False otherwise.
        """
        return self._button_pressed("R1", self.this_controller.getR1Button())

    def r1_button_was_pressed(self) -> bool:
        """
        Check if the R1 button was pressed since the last check.

        :return: True if the R1 button was pressed since the last check, False otherwise.
        """
        return self._button_was_pressed("R1")

    def r2_button_pressed(self) -> bool:
        """
        Check if the R2 button is pressed.

        :return: True if the R2 button is pressed, False otherwise.
        """
        return self._button_pressed("R2", self.this_controller.getR2Button())

    def r2_button_was_pressed(self) -> bool:
        """
        Check if the R2 button was pressed since the last check.

        :return: True if the R2 button was pressed since the last check, False otherwise.
        """
        return self._button_was_pressed("R2")

    def l1_button_pressed(self) -> bool:
        """
        Check if the L1 button is pressed.

        :return: True if the L1 button is pressed, False otherwise.
        """
        return self._button_pressed("L1", self.this_controller.getL1Button())

    def l1_button_was_pressed(self) -> bool:
        """
        Check if the L1 button was pressed since the last check.

        :return: True if the L1 button was pressed since the last check, False otherwise.
        """
        return self._button_was_pressed("L1")

    def l2_button_pressed(self) -> bool:
        """
        Check if the L2 button is pressed.

        :return: True if the L2 button is pressed, False otherwise.
        """
        return self._button_pressed("L2", self.this_controller.getL2Button())

    def l2_button_was_pressed(self) -> bool:
        """
        Check if the L2 button was pressed since the last check.

        :return: True if the L2 button was pressed since the last check, False otherwise.
        """
        return self._button_was_pressed("L2")

    def options_button_pressed(self) -> bool:
        """
        Check if the Options button is pressed.

        :return: True if the Options button is pressed, False otherwise.
        """
        return self._button_pressed("Options", self.this_controller.getOptionsButton())

    def options_button_was_pressed(self) -> bool:
        """
        Check if the Options button was pressed since the last check.

        :return: True if the Options button was pressed since the last check, False otherwise.
        """
        return self._button_was_pressed("Options")

    def share_button_pressed(self) -> bool:
        """
        Check if the Share button is pressed.

        :return: True if the Share button is pressed, False otherwise.
        """
        if self.VERSION == 4:
            return self._button_pressed("Share", self.this_controller.getShareButton())
        else:
            return self._button_pressed("Share", False)

    def share_button_was_pressed(self) -> bool:
        """
        Check if the Share button was pressed since the last check.

        :return: True if the Share button was pressed since the last check, False otherwise.
        """
        return self._button_was_pressed("Share")


class XboxController:
    CORRECT_FOR_DEADBAND: bool
    DEADBAND: float
    PORT: int

    def execute(self) -> None:
        pass

    def setup(self) -> None:
        """
        Setup the Xbox controller.
        This method is called once when the robot is initialized.
        """
        self.this_controller = wpilib.XboxController(self.PORT)
        self.button_was_pressed = {
            "A": False,
            "B": False,
            "X": False,
            "Y": False,
            "DPad_Up": False,
            "DPad_Down": False,
            "DPad_Left": False,
            "DPad_Right": False,
            "Left_Bumper": False,
            "Right_Bumper": False,
            "Left_Trigger": False,
            "Right_Trigger": False,
            "Start": False,
            "Back": False,
        }
        self.button_is_pressed = {
            "A": False,
            "B": False,
            "X": False,
            "Y": False,
            "DPad_Up": False,
            "DPad_Down": False,
            "DPad_Left": False,
            "DPad_Right": False,
            "Left_Bumper": False,
            "Right_Bumper": False,
            "Left_Trigger": False,
            "Right_Trigger": False,
            "Start": False,
            "Back": False,
        }

    def _button_pressed(self, button_name: str, pressed: bool) -> bool:
        """
        Generic method to check if a button is pressed.
        :param button_name: The name of the button to check.
        :param pressed: The current state of the button (True if pressed, False otherwise).
        """
        if pressed:
            self.button_is_pressed[button_name] = True
        else:
            if self.button_is_pressed[button_name]:
                self.button_was_pressed[button_name] = True
            self.button_is_pressed[button_name] = False
        return pressed

    def _button_was_pressed(self, button_name: str) -> bool:
        """
        Generic method to check if a button was pressed since the last check.
        :param button_name: The name of the button to check.
        :return: True if the button was pressed since the last check, False otherwise.
        """
        if self.button_was_pressed[button_name]:
            self.button_was_pressed[button_name] = False
            return True
        return False

    def _corrected_joystick_value(self, raw_value: float) -> float:
        """
        Apply deadband correction to a joystick value.
        :param raw_value: The raw joystick value.
        :return: The corrected joystick value.
        """
        if self.CORRECT_FOR_DEADBAND:
            if abs(raw_value) < self.DEADBAND:
                return 0.0
            return (raw_value / abs(raw_value)) * (
                (abs(raw_value) - self.DEADBAND) / (1 - self.DEADBAND)
            )
        return raw_value

    def capture_button_presses(self) -> None:
        """
        Capture the current state of all buttons to track presses.
        This method should be called at the end of each control loop iteration.
        """
        self.a_button_pressed()
        self.b_button_pressed()
        self.x_button_pressed()
        self.y_button_pressed()
        self.dpad_up_pressed()
        self.dpad_down_pressed()
        self.dpad_left_pressed()
        self.dpad_right_pressed()
        self.left_bumper_pressed()
        self.right_bumper_pressed()
        self.left_trigger_pressed()
        self.right_trigger_pressed()
        self.start_button_pressed()
        self.back_button_pressed()

    def get_joysticks(self) -> tuple[float, float, float, float]:
        """
        Get the joystick values from the Xbox controller.

        :return: A tuple containing the left joystick x, left joystick y, right joystick x, and right joystick y values.
        """
        return (
            self._corrected_joystick_value(self.this_controller.getLeftX()),
            self._corrected_joystick_value(self.this_controller.getLeftY()),
            self._corrected_joystick_value(self.this_controller.getRightX()),
            self._corrected_joystick_value(self.this_controller.getRightY()),
        )

    def a_button_pressed(self) -> bool:
        """
        Check if the A button is pressed.

        :return: True if the A button is pressed, False otherwise.
        """
        return self._button_pressed("A", self.this_controller.getAButton())

    def a_button_was_pressed(self) -> bool:
        """
        Check if the A button was pressed since the last check.

        :return: True if the A button was pressed since the last check, False otherwise.
        """
        return self._button_was_pressed("A")

    def b_button_pressed(self) -> bool:
        """
        Check if the B button is pressed.
        :return: True if the B button is pressed, False otherwise.
        """
        return self._button_pressed("B", self.this_controller.getBButton())

    def b_button_was_pressed(self) -> bool:
        """
        Check if the B button was pressed since the last check.

        :return: True if the B button was pressed since the last check, False otherwise.
        """
        return self._button_was_pressed("B")

    def x_button_pressed(self) -> bool:
        """
        Check if the X button is pressed.

        :return: True if the X button is pressed, False otherwise.
        """
        return self._button_pressed("X", self.this_controller.getXButton())

    def x_button_was_pressed(self) -> bool:
        """
        Check if the X button was pressed since the last check.

        :return: True if the X button was pressed since the last check, False otherwise.
        """
        return self._button_was_pressed("X")

    def y_button_pressed(self) -> bool:
        """
        Check if the Y button is pressed.

        :return: True if the Y button is pressed, False otherwise.
        """
        return self._button_pressed("Y", self.this_controller.getYButton())

    def y_button_was_pressed(self) -> bool:
        """
        Check if the Y button was pressed since the last check.

        :return: True if the Y button was pressed since the last check, False otherwise.
        """
        return self._button_was_pressed("Y")

    def dpad_up_pressed(self) -> bool:
        """
        Check if the D-pad is pressed up.

        :return: True if the D-pad is pressed up, False otherwise.
        """
        return self._button_pressed("DPad_Up", self.this_controller.getPOV() == 0)

    def dpad_up_was_pressed(self) -> bool:
        """
        Check if the D-pad was pressed up since the last check.

        :return: True if the the D-pad was pressed up since the last check, False otherwise.
        """
        return self._button_was_pressed("DPad_Up")

    def dpad_down_pressed(self) -> bool:
        """
        Check if the D-pad is pressed down.

        :return: True if the D-pad is pressed down, False otherwise.
        """
        return self._button_pressed("DPad_Down", self.this_controller.getPOV() == 180)

    def dpad_down_was_pressed(self) -> bool:
        """
        Check if the D-pad was pressed down since the last check.

        :return: True if the the D-pad was pressed down since the last check, False otherwise.
        """
        return self._button_was_pressed("DPad_Down")

    def dpad_left_pressed(self) -> bool:
        """
        Check if the D-pad is pressed left.

        :return: True if the D-pad is pressed left, False otherwise.
        """
        return self._button_pressed("DPad_Left", self.this_controller.getPOV() == 270)

    def dpad_left_was_pressed(self) -> bool:
        """
        Check if the D-pad was pressed left since the last check.

        :return: True if the the D-pad was pressed left since the last check, False otherwise.
        """
        return self._button_was_pressed("DPad_Left")

    def dpad_right_pressed(self) -> bool:
        """
        Check if the D-pad is pressed right.

        :return: True if the D-pad is pressed right, False otherwise.
        """
        return self._button_pressed("DPad_Right", self.this_controller.getPOV() == 90)

    def dpad_right_was_pressed(self) -> bool:
        """
        Check if the D-pad was pressed right since the last check.

        :return: True if the the D-pad was pressed right since the last check, False otherwise.
        """
        return self._button_was_pressed("DPad_Right")

    def left_bumper_pressed(self) -> bool:
        """
        Check if the left bumper is pressed.

        :return: True if the left bumper is pressed, False otherwise.
        """
        return self._button_pressed("Left_Bumper", self.this_controller.getLeftBumper())

    def left_bumper_was_pressed(self) -> bool:
        """
        Check if the left bumper was pressed since the last check.

        :return: True if the left bumper was pressed since the last check, False otherwise.
        """
        return self._button_was_pressed("Left_Bumper")

    def right_bumper_pressed(self) -> bool:
        """
        Check if the right bumper is pressed.

        :return: True if the right bumper is pressed, False otherwise.
        """
        return self._button_pressed(
            "Right_Bumper", self.this_controller.getRightBumper()
        )

    def right_bumper_was_pressed(self) -> bool:
        """
        Check if the right bumper was pressed since the last check.

        :return: True if the right bumper was pressed since the last check, False otherwise.
        """
        return self._button_was_pressed("Right_Bumper")

    def left_trigger_pressed(self) -> float:
        """
        Get the value of the left trigger.

        :return: The value of the left trigger, ranging from 0.0 to 1.0.
        """
        self._button_pressed(
            "Left_Trigger", self.this_controller.getLeftTriggerAxis() >= 0.5
        )
        return self.this_controller.getLeftTriggerAxis()

    def left_trigger_was_pressed(self) -> bool:
        """
        Check if the left trigger was pressed since the last check.

        :return: True if the left trigger was pressed since the last check, False otherwise.
        """
        return self._button_was_pressed("Left_Trigger")

    def right_trigger_pressed(self) -> float:
        """
        Get the value of the right trigger.

        :return: The value of the right trigger, ranging from 0.0 to 1.0.
        """
        self._button_pressed(
            "Right_Trigger", self.this_controller.getRightTriggerAxis() >= 0.5
        )
        return self.this_controller.getRightTriggerAxis()

    def right_trigger_was_pressed(self) -> bool:
        """
        Check if the right trigger was pressed since the last check.

        :return: True if the right trigger was pressed since the last check, False otherwise.
        """
        return self._button_was_pressed("Right_Trigger")

    def start_button_pressed(self) -> bool:
        """
        Check if the start button is pressed.

        :return: True if the start button is pressed, False otherwise.
        """
        return self._button_pressed("Start", self.this_controller.getStartButton())

    def start_button_was_pressed(self) -> bool:
        """
        Check if the start button was pressed since the last check.

        :return: True if the start button was pressed since the last check, False otherwise.
        """
        return self._button_was_pressed("Start")

    def back_button_pressed(self) -> bool:
        """
        Check if the back button is pressed.

        :return: True if the back button is pressed, False otherwise.
        """
        return self._button_pressed("Back", self.this_controller.getBackButton())

    def back_button_was_pressed(self) -> bool:
        """
        Check if the back button was pressed since the last check.

        :return: True if the back button was pressed since the last check, False otherwise.
        """
        return self._button_was_pressed("Back")