"""
GimbalController — Send PID Output to Gimbal Servos
=====================================================
Converts normalized PID signals (-1.0 to 1.0) into servo
angle commands and sends them over the DroneLink serial port.

Author: ByIbos
License: MIT
"""

import struct
import time


class GimbalController:
    """
    Gimbal servo controller over serial link.

    Takes normalized PID output (-1.0 to +1.0) and converts it
    to servo pulse-width values or angle degrees, then sends the
    command to the gimbal via a DroneLink connection.

    Args:
        link: A DroneLink instance for serial communication.
        pan_range: (min_deg, max_deg) for pan axis (default: -90, +90).
        tilt_range: (min_deg, max_deg) for tilt axis (default: -45, +45).
        smooth_factor: Exponential smoothing (0-1, lower = smoother).
            Set to 1.0 for no smoothing (default: 0.3).

    Example:
        >>> from dronelink import DroneLink, GimbalController
        >>> link = DroneLink("COM3")
        >>> gimbal = GimbalController(link)
        >>> gimbal.set_angles(pan=0.5, tilt=-0.3)  # Normalized -1 to 1
        >>> print(f"Current: Pan={gimbal.current_pan:.1f}°, Tilt={gimbal.current_tilt:.1f}°")
    """

    def __init__(self, link, pan_range=(-90, 90), tilt_range=(-45, 45), smooth_factor=0.3):
        self.link = link
        self.pan_range = pan_range
        self.tilt_range = tilt_range
        self.smooth_factor = smooth_factor

        self.current_pan = 0.0   # Current pan angle (degrees)
        self.current_tilt = 0.0  # Current tilt angle (degrees)
        self.target_pan = 0.0
        self.target_tilt = 0.0
        self._last_send_time = 0
        self.min_send_interval = 0.02  # 50 Hz max update rate

    def set_angles(self, pan=0.0, tilt=0.0):
        """
        Set gimbal angles from normalized PID output.

        Args:
            pan: Horizontal signal (-1.0 = full left, +1.0 = full right).
            tilt: Vertical signal (-1.0 = full down, +1.0 = full up).

        Returns:
            bool: True if command was sent successfully.
        """
        # Convert normalized to degrees
        self.target_pan = self._normalize_to_degrees(pan, self.pan_range)
        self.target_tilt = self._normalize_to_degrees(tilt, self.tilt_range)

        # Apply smoothing
        self.current_pan += (self.target_pan - self.current_pan) * self.smooth_factor
        self.current_tilt += (self.target_tilt - self.current_tilt) * self.smooth_factor

        # Rate limiting
        now = time.time()
        if now - self._last_send_time < self.min_send_interval:
            return True  # Skip, too fast

        self._last_send_time = now

        # Pack angles as two int16 values (degrees * 100 for precision)
        pan_int = int(self.current_pan * 100)
        tilt_int = int(self.current_tilt * 100)

        payload = struct.pack('<hh', pan_int, tilt_int)
        return self.link.send_command(
            self.link.CMD_GIMBAL_ANGLE,
            list(payload)
        )

    def center(self):
        """Return gimbal to center position (0°, 0°)."""
        self.current_pan = 0.0
        self.current_tilt = 0.0
        self.target_pan = 0.0
        self.target_tilt = 0.0
        return self.set_angles(0.0, 0.0)

    def _normalize_to_degrees(self, value, angle_range):
        """Convert -1.0...+1.0 to degree range."""
        value = max(-1.0, min(1.0, value))
        mid = (angle_range[0] + angle_range[1]) / 2
        half = (angle_range[1] - angle_range[0]) / 2
        return mid + value * half

    def get_status(self):
        """Return current gimbal state."""
        return {
            'current_pan': self.current_pan,
            'current_tilt': self.current_tilt,
            'target_pan': self.target_pan,
            'target_tilt': self.target_tilt,
        }
