"""
inputs/PygameController.py - Headless Pygame Joystick Provider
"""
import math
from multiprocessing import Queue
import pygame

from inputs.InputProvider import InputProvider, CommandFrame


class PygameController(InputProvider):
    def __init__(self, deadzone: float = 0.2):
        pygame.init()
        pygame.joystick.init()
        
        self.deadzone = deadzone
        self.joysticks = {}
        self.clock = pygame.time.Clock()
        self.current_command = CommandFrame(state="idle", vector_dirmov=[0.0, 0.0, 0.0])

    def _normalize_input(self, x: float, y: float) -> tuple[float, float]:
        magnitude = math.sqrt(x**2 + y**2)
        if magnitude > 1.0:
            x /= magnitude
            y /= magnitude
        return x, y

    def update(self) -> CommandFrame:
        vx, vy, omega = 0.0, 0.0, 0.0
        emote = ""

        # Pump Pygame events in headless mode
        for event in pygame.event.get():
            if event.type == pygame.JOYDEVICEADDED:
                joy = pygame.joystick.Joystick(event.device_index)
                self.joysticks[joy.get_instance_id()] = joy
                print(f"[Controller] Joystick {joy.get_instance_id()} connected: {joy.get_name()}")
            elif event.type == pygame.JOYDEVICEREMOVED:
                if event.instance_id in self.joysticks:
                    print(f"[Controller] Joystick {event.instance_id} disconnected")
                    del self.joysticks[event.instance_id]
            elif event.type == pygame.JOYBUTTONDOWN:
                if event.button == 3:
                    emote = "wave"

        # Read stick values if joystick is connected
        if self.joysticks:
            for joystick in self.joysticks.values():
                raw_vx = -joystick.get_axis(1)  # Forward (+) / Backward (-)
                raw_vy = joystick.get_axis(0)   # Right (+) / Left (-)
                
                num_axes = joystick.get_numaxes()
                raw_omega = joystick.get_axis(3) if num_axes > 3 else joystick.get_axis(2)

                norm_vx, norm_vy = self._normalize_input(raw_vx, raw_vy)

                if abs(raw_omega) >= self.deadzone:
                    omega = raw_omega
                if abs(norm_vx) >= self.deadzone or abs(norm_vy) >= self.deadzone:
                    vx, vy = norm_vx, norm_vy

                # Cardinal snapping
                if 0.8 < vx and -0.2 < vy < 0.2:
                    vx, vy = 1.0, 0.0
                elif vx < -0.8 and -0.2 < vy < 0.2:
                    vx, vy = -1.0, 0.0
                elif -0.2 < vx < 0.2 and 0.8 < vy:
                    vx, vy = 0.0, 1.0
                elif -0.2 < vx < 0.2 and vy < -0.8:
                    vx, vy = 0.0, -1.0

        state = "walking" if (vx != 0.0 or vy != 0.0 or omega != 0.0) else "idle"

        self.current_command = CommandFrame(
            state=state,
            vector_dirmov=[vx, vy, omega],
            emote=emote
        )
        return self.current_command

    def get_command(self) -> CommandFrame:
        return self.update()

    def stop(self) -> None:
        pygame.quit()


def controller_loop(control_queue: Queue):
    provider = PygameController()
    try:
        while True:
            cmd = provider.get_command()
            control_queue.put({
                "state": cmd.state,
                "dirmov": cmd.vector_dirmov,
                "emote": cmd.emote,
                "joysticks_count": len(provider.joysticks)
            })
            provider.clock.tick(30)
    finally:
        provider.stop()