#!/usr/bin/env python3
"""
Windows keyboard control → UDP → RDKx5
Simultaneous keys supported (W+A = forward-left, etc.)

Usage:
    1. pip install keyboard
    2. Run this script on Windows
    3. RDKx5 must be running udp_to_cmdvel node

Controls:
    W/S      : forward / backward
    A/D      : turn left / right
    Q/E      : left-forward / right-forward (single-key diagonal)
    Z/C      : left-backward / right-backward
    Space    : emergency stop
    I/K      : linear  speed +/-
    J/L      : angular speed +/-
    Esc      : quit
"""

import keyboard
import socket
import struct
import time
import sys

# ========== CONFIG ==========
ROBOT_IP = "192.168.10.210"
UDP_PORT = 9999
# ============================

HELP = """
========================================
  Windows Keyboard Control → RDKx5
========================================
  W/S     : forward / backward
  A/D     : turn left / right
  Q/E     : left-forward / right-forward
  Z/C     : left-backward / right-backward
  Space   : STOP
  I/K     : linear  speed +/- (step 0.05)
  J/L     : angular speed +/- (step 0.1)
  Esc     : quit
========================================
  Current: lin=%.2f  ang=%.2f
"""


def main():
    sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)

    lin_speed = 0.2
    ang_speed = 0.5
    lin_step = 0.05
    ang_step = 0.1

    def print_help():
        print(HELP % (lin_speed, ang_speed))

    print("Connecting to %s:%d ..." % (ROBOT_IP, UDP_PORT))
    print_help()

    while True:
        if keyboard.is_pressed('esc'):
            sock.sendto(struct.pack('dd', 0.0, 0.0), (ROBOT_IP, UDP_PORT))
            print("\nQuit.")
            break

        # Speed adjustments (one-shot, debounced by keyboard library)
        if keyboard.is_pressed('i'):
            lin_speed = round(lin_speed + lin_step, 2)
            print_help()
            time.sleep(0.15)
            continue
        if keyboard.is_pressed('k'):
            lin_speed = round(max(0.0, lin_speed - lin_step), 2)
            print_help()
            time.sleep(0.15)
            continue
        if keyboard.is_pressed('j'):
            ang_speed = round(ang_speed + ang_step, 2)
            print_help()
            time.sleep(0.15)
            continue
        if keyboard.is_pressed('l'):
            ang_speed = round(max(0.0, ang_speed - ang_step), 2)
            print_help()
            time.sleep(0.15)
            continue

        # Emergency stop
        if keyboard.is_pressed('space'):
            sock.sendto(struct.pack('dd', 0.0, 0.0), (ROBOT_IP, UDP_PORT))
            print("*** STOP ***")
            time.sleep(0.05)
            continue

        # Combined movement: multi-key support
        lin = 0.0
        ang = 0.0

        if keyboard.is_pressed('w'):
            lin += lin_speed
        if keyboard.is_pressed('s'):
            lin -= lin_speed
        if keyboard.is_pressed('a'):
            ang += ang_speed
        if keyboard.is_pressed('d'):
            ang -= ang_speed

        # Single-key diagonals (override if used)
        if keyboard.is_pressed('q'):
            lin, ang = lin_speed, ang_speed
        if keyboard.is_pressed('e'):
            lin, ang = lin_speed, -ang_speed
        if keyboard.is_pressed('z'):
            lin, ang = -lin_speed, ang_speed
        if keyboard.is_pressed('c'):
            lin, ang = -lin_speed, -ang_speed

        sock.sendto(struct.pack('dd', round(lin, 3), round(ang, 3)),
                    (ROBOT_IP, UDP_PORT))

        time.sleep(0.05)  # 20Hz


if __name__ == "__main__":
    print("Installing keyboard library if needed...")
    try:
        import keyboard
    except ImportError:
        print("ERROR: 'keyboard' library not installed.")
        print("Run: pip install keyboard")
        sys.exit(1)

    main()