"""
Controller script for Beach Buggy with full drivetrain
"""
import os
import openplx
from agxPythonModules.utils.environment import init_app, application, root
from agxPythonModules.utils.callbacks import KeyboardCallback
from openplx.Physics.Signals import RealInputSignal, IntInputSignal
import agx
import agxSDK

# Class to manage vehicle maneuvering using signal interface
class VehicleManeuvering:  # pylint: disable=too-many-instance-attributes, too-many-arguments, too-many-positional-arguments
    def __init__(
        self,
        signal_interface: openplx.Physics.Signals.SignalInterface,
        input_queue,
    ):
        """
        Initialize the vehicle maneuvering class with input signals control.

        :param input_queue: Queue to send signals to the simulation
        :param signal_interface: The signal interface keeping track of our signals
        """
        super().__init__()

        self.inputs = signal_interface.getInputs()

        self.input_queue = input_queue

        self.engine_input_fraction = 0
        self.gear_box_selected_gear = 0
        self.steering_angle = 0

    def send_signals(self):
        """
        Send signals
        """
        self.input_queue.send(
            RealInputSignal.create(self.engine_input_fraction, self.inputs["drivetrain_throttle_input"])
        )
        self.input_queue.send(
            IntInputSignal.create(self.gear_box_selected_gear, self.inputs["drivetrain_gear_selection_input"])
        )
        self.input_queue.send(
            RealInputSignal.create(self.steering_angle, self.inputs["steering_left_input"])
        )
        self.input_queue.send(
            RealInputSignal.create(self.steering_angle, self.inputs["steering_right_input"])
        )

    def forward(self):
        """
        Switch gears and increase engine throttle to start forward movement and send signals.
        """
        if self.engine_input_fraction < 0.90:
            self.engine_input_fraction += 0.01
        self.gear_box_selected_gear = 1
        self.send_signals()

    def backward(self):
        """
        Switch gears and decrease engine throttle to start backward movement and send signals.
        """
        if self.engine_input_fraction < 0.90:
            self.engine_input_fraction += 0.01
        self.gear_box_selected_gear = -1
        self.send_signals()

    def left(self):
        """
        Handle left steering and send signals.
        """
        if self.steering_angle > -0.5:
            self.steering_angle -= 0.01
        self.send_signals()

    def right(self):
        """
        Handle right steering and send signals.
        """
        if self.steering_angle < 0.5:
            self.steering_angle += 0.01
        self.send_signals()

    def release(self):
        """
        Set brake and clutch inputs to 0 and set gear to neutral.
        """
        self.engine_input_fraction = 0
        self.gear_box_selected_gear = 0

        self.send_signals()

    def handle_forward(self, key) -> bool:
        """
        Handle forward key press/release events.

        :param key: Key event object
        :return: True if key handling was successful
        """
        if key.down:
            self.forward()  # Move forward on key press
        else:
            self.release()  # Release on key release
        return True

    def handle_backward(self, key) -> bool:
        """
        Handle backward key press/release events.

        :param key: Key event object
        :return: True if key handling was successful
        """
        if key.down:
            self.backward()  # Move backward on key press
        else:
            self.release()  # Release on key release
        return True

    def handle_left(self, key) -> bool:
        """
        Handle left key press/release events.

        :param key: Key event object
        :return: True if key handling was successful
        """
        if key.down:
            self.left()  # Steer left on key press
        else:
            self.release()  # Release on key release
        return True

    def handle_right(self, key) -> bool:
        """
        Handle right key press/release events.

        :param key: Key event object
        :return: True if key handling was successful
        """
        if key.down:
            self.right()  # Steer right on key press
        else:
            self.release()  # Release on key release
        return True

    def setup_keyboard_listener(self):
        """
        Set up keyboard listeners for forward and backward controls.
        """
        KeyboardCallback.bind(
            name="forward",
            key=agxSDK.GuiEventListener.KEY_Up,
            callback=self.handle_forward,
        )
        KeyboardCallback.bind(
            name="backward",
            key=agxSDK.GuiEventListener.KEY_Down,
            callback=self.handle_backward,
        )
        KeyboardCallback.bind(
            name="left",
            key=agxSDK.GuiEventListener.KEY_Left,
            callback=self.handle_left,
        )
        KeyboardCallback.bind(
            name="right",
            key=agxSDK.GuiEventListener.KEY_Right,
            callback=self.handle_right,
        )

def initialize_scene_decorator():
    """
    Set the initial camera position in the scene.
    """
    camera_data = application().getCameraData()
    camera_data.eye = agx.Vec3(-5.4735, -29.3909, 5.1350)
    camera_data.center = agx.Vec3(-0.3179, -5.7875, 0.6757)
    camera_data.up = agx.Vec3(0.0536, 0.1741, 0.9833)
    camera_data.nearClippingPlane = 0.1
    camera_data.farClippingPlane = 5000

    application().setCameraHome(camera_data.eye, camera_data.center, camera_data.up)
    application().applyCameraData(camera_data)


def buildScene():  # pylint: disable=invalid-name
    """
    Load the OpenPLX scene and set up the vehicle maneuvering system.
    """

    initialize_scene_decorator()


    # Get the current script directory and load the OpenPLX scene
    file_dir = os.path.dirname(os.path.abspath(__file__))
    result = application().loadOpenPlxFile(
        os.path.join(file_dir, "Scenes/SceneBeachBuggyFullDrivetrain.openplx"),
        root()
    )

    openplx_scene = result.scene()
    signal_interface = openplx_scene.getObject("beach_buggy.signal_interface")
    assert signal_interface is not None, "Signal interface not found in the scene."
    assert signal_interface.getInputs()["steering_left_input"]
    assert signal_interface.getInputs()["drivetrain_throttle_input"]

    # Initialize vehicle maneuvering and set up input listeners
    vehicle_maneuvering = VehicleManeuvering(
        signal_interface, result.getInputSignalQueue()
    )
    vehicle_maneuvering.setup_keyboard_listener()


init = init_app(name="__main__", scenes=[(buildScene, "1")], autoStepping=False)
