import pygame
from pydualsense import *
     
BATTERY_STATE = {
    "0": "Discharging",
    "1": "Charging",
    "2": "Full",
    "11": "Not charging",
    "15": "Error",
    "10": "Temp or voltage out of range"
    }

# Initialize pygame
pygame.init()
screen = pygame.display.set_mode((800, 600))
pygame.display.set_caption("DualSense Demo with Pygame")
font = pygame.font.SysFont(None, 48)
clock = pygame.time.Clock()

# Initialize DualSense
ds = pydualsense()
ds.init()

# Set initial LED color
ds.light.setColorI(0, 100, 255)

# Configure adaptive trigger
ds.triggerL.setMode(TriggerModes.Rigid)
ds.triggerL.setForce(1, 255)

running = True
while running:
    screen.fill((30, 30, 30))

    # Read inputs from DualSense
    text_lines = [
        f"LX: {ds.state.LX:.2f}, LY: {ds.state.LY:.2f}",
        f"RX: {ds.state.RX:.2f}, RY: {ds.state.RY:.2f}",
        f"L2: {ds.state.L2:.2f}, R2: {ds.state.R2:.2f}",
        f"Accel: {ds.state.accelerometer.X:.2f}, {ds.state.accelerometer.Y:.2f}, {ds.state.accelerometer.Z:.2f}",
        f"Gyro: {ds.state.gyro.Roll:.2f}, {ds.state.gyro.Pitch:.2f}, {ds.state.gyro.Yaw:.2f}",
        f"Power: {ds.battery.Level:4}%, State: {BATTERY_STATE[str(ds.battery.State)]}"
    ]

    for i, line in enumerate(text_lines):
        text = font.render(line, True, (255, 255, 255))
        screen.blit(text, (20, 40 + i * 50))

    # Button-based actions
    if ds.state.square:
        ds.light.setColorI(255, 0, 0)  # Red
    elif ds.state.triangle:
        ds.light.setColorI(0, 255, 0)  # Green
    elif ds.state.circle:
        ds.light.setColorI(0, 0, 255)  # Blue
    elif ds.state.cross:
        ds.light.setColorI(255, 255, 255)  # White

    #ds.setLeftMotor(200)
    #ds.setRightMotor(200)  # Heavy rumble

    # Handle pygame events
    for event in pygame.event.get():
        if event.type == pygame.QUIT:  # Exit on window close or PS button
            running = False

    if ds.state.ps:
        running = False
        
    pygame.display.flip()
    clock.tick(60)

# Clean up
ds.setLeftMotor(0)
ds.setRightMotor(0)
ds.light.setColorI(0, 0, 255)
ds.close()
pygame.quit()