#!/usr/bin/env python3
import json
import math
import os
import subprocess
import sys
import time


WHEELS = ('fl', 'fr', 'cl', 'cr', 'rl', 'rr')

WHEEL_LABELS = {
    'fl': 'Vorne links',
    'fr': 'Vorne rechts',
    'cl': 'Mitte links',
    'cr': 'Mitte rechts',
    'rl': 'Hinten links',
    'rr': 'Hinten rechts',
}

WHEEL_DIRECTIONS = {
    'fl': -1,
    'fr': 1,
    'cl': -1,
    'cr': 1,
    'rl': -1,
    'rr': 1,
}

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

STEER_NEUTRAL_KEYS = {
    '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',
}

DRIVE_PIN_KEYS = {
    'fl': 'pin_drive_fl',
    'fr': 'pin_drive_fr',
    'cl': 'pin_drive_cl',
    'cr': 'pin_drive_cr',
    'rl': 'pin_drive_rl',
    'rr': 'pin_drive_rr',
}

DRIVE_NEUTRAL_KEYS = {
    'fl': 'drive_pwm_neutral_fl',
    'fr': 'drive_pwm_neutral_fr',
    'cl': 'drive_pwm_neutral_cl',
    'cr': 'drive_pwm_neutral_cr',
    'rl': 'drive_pwm_neutral_rl',
    'rr': 'drive_pwm_neutral_rr',
}

ACTION_SETTINGS = {
    'steer_left': {
        'kind': 'steer',
        'angle': -35,
        'duration': 0.6,
        'label': 'Lenkung links',
    },
    'steer_right': {
        'kind': 'steer',
        'angle': 35,
        'duration': 0.6,
        'label': 'Lenkung rechts',
    },
    'drive_forward': {
        'kind': 'drive',
        'speed': 35,
        'duration': 0.7,
        'label': 'Vorwärts',
    },
    'drive_backward': {
        'kind': 'drive',
        'speed': -35,
        'duration': 0.7,
        'label': 'Rückwärts',
    },
}

CONTAINER_NAME = 'exomy_autostart'
CONTAINER_SCRIPT_PATH = '/root/exomy_ws/src/exomy/scripts/admin_motor_test.py'


class PCA9685Direct:
    MODE1 = 0x00
    PRESCALE = 0xFE
    LED0_ON_L = 0x06

    def __init__(self, busnum=1, address=0x40):
        try:
            import smbus
        except ImportError as exc:
            try:
                import smbus2 as smbus
            except ImportError as inner_exc:
                raise RuntimeError('smbus ist nicht installiert.') from inner_exc

        self.address = address
        self.bus = smbus.SMBus(busnum)
        self.write8(self.MODE1, 0x00)
        time.sleep(0.005)

    def write8(self, reg, value):
        self.bus.write_byte_data(self.address, reg, value & 0xFF)

    def read8(self, reg):
        return self.bus.read_byte_data(self.address, reg)

    def set_pwm_freq(self, freq_hz):
        prescaleval = 25000000.0
        prescaleval /= 4096.0
        prescaleval /= float(freq_hz)
        prescaleval -= 1.0
        prescale = int(math.floor(prescaleval + 0.5))

        oldmode = self.read8(self.MODE1)
        sleepmode = (oldmode & 0x7F) | 0x10
        self.write8(self.MODE1, sleepmode)
        self.write8(self.PRESCALE, prescale)
        self.write8(self.MODE1, oldmode)
        time.sleep(0.005)
        self.write8(self.MODE1, oldmode | 0xA1)

    def set_pwm(self, channel, on, off):
        base = self.LED0_ON_L + 4 * int(channel)
        self.write8(base, on & 0xFF)
        self.write8(base + 1, (on >> 8) & 0xFF)
        self.write8(base + 2, off & 0xFF)
        self.write8(base + 3, (off >> 8) & 0xFF)


def _load_config(config_path):
    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()
    return config


def _update_config_values(config_path, updates):
    output_lines = []
    with open(config_path, 'r', encoding='utf-8') as handle:
        for raw_line in handle:
            line = raw_line
            stripped = raw_line.strip()
            if stripped and not stripped.startswith('#') and ':' in raw_line:
                key = raw_line.split(':', 1)[0].strip()
                if key in updates:
                    line = f'{key}: {updates[key]}\n'
            output_lines.append(line)

    with open(config_path, 'w', encoding='utf-8') as handle:
        handle.writelines(output_lines)


class MotorTester:
    def __init__(self, config_path=None):
        try:
            import Adafruit_PCA9685
            self.pwm = Adafruit_PCA9685.PCA9685()
        except ImportError:
            self.pwm = PCA9685Direct(busnum=1)

        if config_path is None:
            config_path = os.path.join(os.path.dirname(__file__), '..', 'config', 'exomy.yaml')

        if not os.path.exists(config_path):
            raise FileNotFoundError(f'Konfigurationsdatei nicht gefunden: {config_path}')

        self.config_path = os.path.abspath(config_path)
        self.config = _load_config(self.config_path)
        self.pwm.set_pwm_freq(50)
        self.steer_pwm_range = int(self.config['steer_pwm_range'])
        self.drive_pwm_neutral = {
            wheel: int(self.config[DRIVE_NEUTRAL_KEYS[wheel]])
            for wheel in WHEELS
        }
        self.drive_pwm_range = int(self.config['drive_pwm_range'])

    def _steer_pin(self, wheel):
        return int(self.config[STEER_PIN_KEYS[wheel]])

    def _steer_neutral(self, wheel):
        return int(self.config[STEER_NEUTRAL_KEYS[wheel]])

    def _drive_pin(self, wheel):
        return int(self.config[DRIVE_PIN_KEYS[wheel]])

    def _drive_neutral(self, wheel):
        return int(self.drive_pwm_neutral[wheel])

    def set_drive_neutral_pwm(self, values):
        for wheel in WHEELS:
            self.pwm.set_pwm(self._drive_pin(wheel), 0, int(values[wheel]))
            time.sleep(0.03)

    def center_all_steering(self):
        for wheel in WHEELS:
            self.pwm.set_pwm(self._steer_pin(wheel), 0, self._steer_neutral(wheel))
            time.sleep(0.03)

    def stop_all_driving(self):
        for wheel in WHEELS:
            self.pwm.set_pwm(self._drive_pin(wheel), 0, self._drive_neutral(wheel))
            time.sleep(0.03)

    def set_steering_angle(self, wheel, angle):
        duty_cycle = int(self._steer_neutral(wheel) + angle / 90.0 * self.steer_pwm_range)
        self.pwm.set_pwm(self._steer_pin(wheel), 0, duty_cycle)

    def set_steering_neutral(self, wheel):
        self.pwm.set_pwm(self._steer_pin(wheel), 0, self._steer_neutral(wheel))

    def set_steering_neutral_pwm(self, wheel, pwm_value):
        self.pwm.set_pwm(self._steer_pin(wheel), 0, int(pwm_value))

    def set_steering_percent(self, wheel, signed_percent):
        percent = max(-100.0, min(100.0, float(signed_percent)))
        duty_cycle = int(self._steer_neutral(wheel) + percent / 100.0 * self.steer_pwm_range)
        self.pwm.set_pwm(self._steer_pin(wheel), 0, duty_cycle)

    def set_all_steering_angle(self, angle):
        for wheel in WHEELS:
            self.set_steering_angle(wheel, angle)
            time.sleep(0.03)

    def set_all_steering_neutral(self):
        for wheel in WHEELS:
            self.set_steering_neutral(wheel)
            time.sleep(0.03)

    def test_steering(self, wheel, angle, duration):
        self.set_steering_angle(wheel, angle)
        time.sleep(duration)
        self.set_steering_neutral(wheel)

    def test_driving(self, wheel, speed, duration):
        self.pwm.set_pwm(self._steer_pin(wheel), 0, self._steer_neutral(wheel))
        duty_cycle = int(
            self._drive_neutral(wheel) +
            speed / 100.0 * self.drive_pwm_range * WHEEL_DIRECTIONS[wheel]
        )
        self.pwm.set_pwm(self._drive_pin(wheel), 0, duty_cycle)
        time.sleep(duration)
        self.pwm.set_pwm(self._drive_pin(wheel), 0, self._drive_neutral(wheel))

    def execute(self, wheel, action):
        if wheel not in WHEELS:
            raise ValueError('Unbekanntes Rad.')

        if action not in ACTION_SETTINGS:
            raise ValueError('Unbekannte Testaktion.')

        settings = ACTION_SETTINGS[action]

        self.stop_all_driving()

        if settings['kind'] == 'steer':
            self.test_steering(wheel, settings['angle'], settings['duration'])
        else:
            self.test_driving(wheel, settings['speed'], settings['duration'])

        return {
            'wheel': wheel,
            'wheel_label': WHEEL_LABELS[wheel],
            'action': action,
            'action_label': settings['label'],
        }

    def hold_steering(self, wheel, angle):
        if wheel not in WHEELS:
            raise ValueError('Unbekanntes Rad.')

        self.stop_all_driving()
        self.set_steering_angle(wheel, angle)
        return {
            'scope': 'single',
            'wheel': wheel,
            'wheel_label': WHEEL_LABELS[wheel],
            'angle': angle,
        }

    def hold_steering_percent(self, wheel, percent, direction):
        if wheel not in WHEELS:
            raise ValueError('Unbekanntes Rad.')

        if direction not in ('left', 'right'):
            raise ValueError('Unbekannte Richtung.')

        clamped_percent = max(0.0, min(100.0, float(percent)))
        signed_percent = -clamped_percent if direction == 'left' else clamped_percent

        self.stop_all_driving()
        self.set_steering_percent(wheel, signed_percent)
        return {
            'scope': 'single',
            'wheel': wheel,
            'wheel_label': WHEEL_LABELS[wheel],
            'percent': clamped_percent,
            'direction': direction,
            'signed_percent': signed_percent,
        }

    def hold_all_steering(self, angle):
        self.stop_all_driving()
        self.set_all_steering_angle(angle)
        return {
            'scope': 'all',
            'wheel': 'all',
            'wheel_label': 'Alle Räder',
            'angle': angle,
        }

    def neutral_single_steering(self, wheel):
        if wheel not in WHEELS:
            raise ValueError('Unbekanntes Rad.')

        self.stop_all_driving()
        self.set_steering_neutral(wheel)
        return {
            'scope': 'single',
            'wheel': wheel,
            'wheel_label': WHEEL_LABELS[wheel],
            'angle': 0,
        }

    def neutral_all_steering(self):
        self.stop_all_driving()
        self.set_all_steering_neutral()
        return {
            'scope': 'all',
            'wheel': 'all',
            'wheel_label': 'Alle Räder',
            'angle': 0,
        }


def run_motor_test(wheel, action, config_path=None):
    tester = MotorTester(config_path=config_path)
    return tester.execute(wheel.lower(), action)


def hold_single_steering(wheel, angle, config_path=None):
    tester = MotorTester(config_path=config_path)
    return tester.hold_steering(wheel.lower(), angle)


def hold_all_steering(angle, config_path=None):
    tester = MotorTester(config_path=config_path)
    return tester.hold_all_steering(angle)


def hold_single_steering_percent(wheel, percent, direction, config_path=None):
    tester = MotorTester(config_path=config_path)
    return tester.hold_steering_percent(wheel.lower(), percent, direction)


def neutral_single_steering(wheel, config_path=None):
    tester = MotorTester(config_path=config_path)
    return tester.neutral_single_steering(wheel.lower())


def neutral_all_steering(config_path=None):
    tester = MotorTester(config_path=config_path)
    return tester.neutral_all_steering()


def get_steering_neutral_values(config_path=None):
    tester = MotorTester(config_path=config_path)
    values = {}
    for wheel in WHEELS:
        key = STEER_NEUTRAL_KEYS[wheel]
        values[wheel] = {
            'wheel': wheel,
            'wheel_label': WHEEL_LABELS[wheel],
            'key': key,
            'value': int(tester.config[key]),
            'pin': tester._steer_pin(wheel),
        }
    return {
        'config_path': tester.config_path,
        'values': values,
    }


def preview_steering_neutral_value(wheel, pwm_value, config_path=None):
    tester = MotorTester(config_path=config_path)
    if wheel not in WHEELS:
        raise ValueError('Unbekanntes Rad.')

    tester.stop_all_driving()
    tester.set_steering_neutral_pwm(wheel, int(pwm_value))
    return {
        'wheel': wheel,
        'wheel_label': WHEEL_LABELS[wheel],
        'value': int(pwm_value),
        'pin': tester._steer_pin(wheel),
    }


def save_steering_neutral_values(values, config_path=None):
    tester = MotorTester(config_path=config_path)
    updates = {}

    for wheel in WHEELS:
        key = STEER_NEUTRAL_KEYS[wheel]
        if key not in values:
            raise ValueError(f'Neutralwert fehlt: {key}')
        updates[key] = int(values[key])

    _update_config_values(tester.config_path, updates)
    tester.config = _load_config(tester.config_path)
    tester.stop_all_driving()
    tester.center_all_steering()

    return get_steering_neutral_values(config_path=tester.config_path)


def get_drive_neutral_value(config_path=None):
    tester = MotorTester(config_path=config_path)
    values = {}
    for wheel in WHEELS:
        key = DRIVE_NEUTRAL_KEYS[wheel]
        values[wheel] = {
            'wheel': wheel,
            'wheel_label': WHEEL_LABELS[wheel],
            'key': key,
            'value': int(tester.config[key]),
            'pin': tester._drive_pin(wheel),
        }
    return {
        'config_path': tester.config_path,
        'values': values,
    }


def preview_drive_neutral_value(values, config_path=None):
    tester = MotorTester(config_path=config_path)
    preview_values = {}
    for wheel in WHEELS:
        if wheel not in values:
            raise ValueError(f'Fahr-Neutralwert fehlt: {DRIVE_NEUTRAL_KEYS[wheel]}')
        preview_values[wheel] = int(values[wheel])

    tester.center_all_steering()
    tester.set_drive_neutral_pwm(preview_values)
    return {
        'values': {
            wheel: {
                'wheel': wheel,
                'wheel_label': WHEEL_LABELS[wheel],
                'key': DRIVE_NEUTRAL_KEYS[wheel],
                'value': preview_values[wheel],
                'pin': tester._drive_pin(wheel),
            }
            for wheel in WHEELS
        },
    }


def save_drive_neutral_value(values, config_path=None):
    tester = MotorTester(config_path=config_path)
    updates = {}
    for wheel in WHEELS:
        key = DRIVE_NEUTRAL_KEYS[wheel]
        if key not in values:
            raise ValueError(f'Fahr-Neutralwert fehlt: {key}')
        updates[key] = int(values[key])

    _update_config_values(tester.config_path, updates)
    tester.config = _load_config(tester.config_path)
    tester.drive_pwm_neutral = {
        wheel: int(tester.config[DRIVE_NEUTRAL_KEYS[wheel]])
        for wheel in WHEELS
    }
    tester.center_all_steering()
    tester.set_drive_neutral_pwm(tester.drive_pwm_neutral)
    return get_drive_neutral_value(config_path=tester.config_path)


def run_motor_test_in_container(wheel, action):
    command = [
        'docker',
        'exec',
        CONTAINER_NAME,
        'python',
        CONTAINER_SCRIPT_PATH,
        wheel.lower(),
        action,
    ]
    result = subprocess.run(command, capture_output=True, text=True, check=False)
    if result.returncode != 0:
        stderr = result.stderr.strip()
        stdout = result.stdout.strip()
        message = stderr or stdout or 'Motortest im Container fehlgeschlagen'
        raise RuntimeError(message)

    try:
        return json.loads(result.stdout)
    except json.JSONDecodeError as exc:
        raise RuntimeError('Antwort des Container-Motortests war ungültig') from exc


def main():
    if len(sys.argv) != 3:
        print('Aufruf: admin_motor_test.py <fl|fr|cl|cr|rl|rr> <steer_left|steer_right|drive_forward|drive_backward>')
        return 1

    wheel = sys.argv[1].lower()
    action = sys.argv[2]
    result = run_motor_test(wheel, action, config_path=None)
    print(json.dumps(result))
    return 0


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