#!/usr/bin/env python3
import sys

try:
    import Adafruit_PCA9685
except ImportError:
    print("Adafruit_PCA9685 ist nicht installiert. Dieses Skript ist fuer den Raspberry/Container gedacht.")
    sys.exit(1)


WHEEL_TO_PIN = {
    "fl": "pin_steer_fl",
    "fr": "pin_steer_fr",
    "cl": "pin_steer_cl",
    "cr": "pin_steer_cr",
    "rl": "pin_steer_rl",
    "rr": "pin_steer_rr",
}

WHEEL_TO_NEUTRAL = {
    "fl": "steer_pwm_neutral_fl",
    "fr": "steer_pwm_neutral_fr",
    "cl": "steer_pwm_neutral_cl",
    "cr": "steer_pwm_neutral_cr",
    "rl": "steer_pwm_neutral_rl",
    "rr": "steer_pwm_neutral_rr",
}
def main() -> int:
    if len(sys.argv) != 4:
        print("Aufruf: python3 test_single_steer_wheel.py <config.yaml> <fl|fr|cl|cr|rl|rr> <winkel>")
        return 1

    config_path = sys.argv[1]
    wheel = sys.argv[2].lower()
    angle = float(sys.argv[3])

    if wheel not in WHEEL_TO_PIN:
        print("Unbekanntes Rad:", wheel)
        return 1

    config = {}
    with open(config_path, "r", encoding="utf-8") as handle:
        for raw_line in handle:
            line = raw_line.strip()
            if not line or line.startswith("#") or ":" not in line:
                continue
            key, value = line.split(":", 1)
            config[key.strip()] = value.strip()

    pin = int(config[WHEEL_TO_PIN[wheel]])
    neutral = int(config[WHEEL_TO_NEUTRAL[wheel]])
    pwm_range = int(config["steer_pwm_range"])
    pwm_value = int(neutral + angle / 90.0 * pwm_range)

    print("wheel:", wheel)
    print("pin:", pin)
    print("neutral:", neutral)
    print("angle:", angle)
    print("pwm:", pwm_value)

    pwm = Adafruit_PCA9685.PCA9685(busnum=1)
    pwm.set_pwm_freq(50)
    pwm.set_pwm(pin, 0, pwm_value)
    return 0


if __name__ == "__main__":
    raise SystemExit(main())