"""
Dreht den Rover auf der Stelle zu einem Ziel-Heading.

Öffentliche API:
    set_publish_fn(fn)   wird von exomy_admin_api nach ROS-Connect gesetzt
    start_rotate(target_deg, get_heading_fn) -> dict
    stop_rotate()
    get_rotate_status() -> dict
"""

import threading
import time

_POINT_TURN = 2    # LocomotionMode.POINT_TURN
_ACKERMANN  = 1    # LocomotionMode.ACKERMANN (Standard-Rückkehrmodus)
_DONE_DEG   = 3.0  # Toleranz in Grad
_VEL        = 20   # konstante Drehgeschwindigkeit (0–100), direkte Motoransteuerung
_INTERVAL   = 0.02 # Sekunden zwischen Regelschritten (50 Hz)

_lock       = threading.Lock()
_thread     = None
_stop_event = threading.Event()
_publish_fn = None

_status = {
    'running':     False,
    'target_deg':  None,
    'current_deg': None,
    'error_deg':   None,
    'result':      'idle',
    'message':     '',
}


def set_publish_fn(fn):
    global _publish_fn
    _publish_fn = fn


def _publish(locomotion_mode, vel, steering):
    fn = _publish_fn
    if fn is not None:
        fn(locomotion_mode=locomotion_mode, vel=vel, steering=steering)


def _shortest_error(current, target):
    """Vorzeichen: positiv = rechtsdrehung nötig, negativ = linksdrehung."""
    return (target - current + 540) % 360 - 180


def _restore(restore_mode):
    _publish(restore_mode, vel=0, steering=0)


def _control_loop(target_deg, get_heading_fn, restore_mode):
    global _status

    try:
        while not _stop_event.is_set():
            heading = get_heading_fn()
            if heading is None:
                time.sleep(0.05)
                continue

            error = _shortest_error(heading, target_deg)

            with _lock:
                _status['current_deg'] = round(heading, 1)
                _status['error_deg']   = round(error, 1)

            if abs(error) <= _DONE_DEG:
                _restore(restore_mode)
                with _lock:
                    _status['running'] = False
                    _status['result']  = 'done'
                    _status['message'] = 'Ziel erreicht'
                return

            vel = _VEL if error > 0 else -_VEL
            _publish(_POINT_TURN, vel=vel, steering=0)
            time.sleep(_INTERVAL)

        _restore(restore_mode)
        with _lock:
            _status['running'] = False
            _status['result']  = 'aborted'
            _status['message'] = 'Abgebrochen'

    except Exception as exc:
        _restore(restore_mode)
        with _lock:
            _status['running'] = False
            _status['result']  = 'error'
            _status['message'] = str(exc)


def start_rotate(target_deg, get_heading_fn):
    global _thread, _status

    target_deg = float(target_deg) % 360
    stop_rotate()

    _stop_event.clear()
    with _lock:
        _status = {
            'running':     True,
            'target_deg':  round(target_deg, 1),
            'current_deg': None,
            'error_deg':   None,
            'result':      'running',
            'message':     'Drehe zu {0:.0f}°'.format(target_deg),
        }

    _thread = threading.Thread(
        target=_control_loop,
        args=(target_deg, get_heading_fn),
        daemon=True,
    )
    _thread.start()
    return get_rotate_status()


def stop_rotate():
    global _thread
    if _thread is not None and _thread.is_alive():
        _stop_event.set()
        _thread.join(timeout=2.0)
    _thread = None


def get_rotate_status():
    with _lock:
        return dict(_status)