#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
Laeuft INNERHALB des Docker-Containers mit nativem rospy.
Dreht den Rover auf der Stelle zu einem Ziel-Heading und beendet sich.

Aufruf:
    python3 auto_rotate_ros.py <target_deg> <restore_mode> <heading_offset_deg>

Exit-Codes:
    0 = Ziel erreicht
    1 = Fehler
"""

import math
import signal
import sys
import time

import rospy
from exomy.msg import RoverCommand
from sensor_msgs.msg import Imu

_POINT_TURN = 2
_DONE_DEG   = float(sys.argv[4]) if len(sys.argv) > 4 else 1.0
_VEL        = int(sys.argv[5])   if len(sys.argv) > 5 else 20

target_deg      = float(sys.argv[1]) % 360
restore_mode    = int(sys.argv[2])
heading_offset  = float(sys.argv[3])

_running   = True
_publisher = None
_heading   = None


def _normalize(deg):
    return (float(deg) % 360 + 360) % 360


def _shortest_error(current, target):
    return (target - current + 540) % 360 - 180


def _on_imu(msg):
    global _heading
    o = msg.orientation
    x, y, z, w = o.x, o.y, o.z, o.w
    siny = 2.0 * (w * z + x * y)
    cosy = 1.0 - 2.0 * (y * y + z * z)
    yaw_deg = math.degrees(math.atan2(siny, cosy))
    _heading = _normalize(yaw_deg + heading_offset)


def _publish(locomotion_mode, vel, steering=0):
    if _publisher is None:
        return
    cmd = RoverCommand()
    cmd.connected       = True
    cmd.motors_enabled  = True
    cmd.locomotion_mode = locomotion_mode
    cmd.vel             = vel
    cmd.steering        = steering
    _publisher.publish(cmd)


def _shutdown(signum, frame):
    global _running
    _running = False


def main():
    global _publisher, _running

    signal.signal(signal.SIGTERM, _shutdown)
    signal.signal(signal.SIGINT,  _shutdown)

    rospy.init_node('auto_rotate', anonymous=True, disable_signals=True)

    _publisher = rospy.Publisher('/rover_command', RoverCommand, queue_size=1)
    rospy.Subscriber('/imu/data', Imu, _on_imu, queue_size=1)

    # Kurz warten bis IMU-Daten ankommen
    deadline = time.time() + 5.0
    while _heading is None and time.time() < deadline and _running:
        time.sleep(0.05)

    if _heading is None:
        rospy.logerr('auto_rotate: keine IMU-Daten')
        _publish(restore_mode, vel=0)
        sys.exit(1)

    rate = rospy.Rate(50)  # 50 Hz, passend zum PWM-Chip

    while _running and not rospy.is_shutdown():
        error = _shortest_error(_heading, target_deg)

        if abs(error) <= _DONE_DEG:
            _publish(restore_mode, vel=0)
            sys.exit(0)

        # Richtung wie Joystick: immer positiver vel, steering ±180 für Richtung
        # steering=0   → Linksdrehung (deg < 85)
        # steering=180 → Rechtsdrehung (deg >= 85)
        if error > 0:
            _publish(_POINT_TURN, vel=_VEL, steering=0)
        else:
            _publish(_POINT_TURN, vel=_VEL, steering=180)
        rate.sleep()

    # Abbruch per Signal
    _publish(restore_mode, vel=0)
    sys.exit(0)


if __name__ == '__main__':
    main()