#!/usr/bin/env python3
"""
Real-time Gimbal Angle Monitor
Professional-grade implementation for SIP series gimbal communication
Adheres 100% to the documented protocol specifications
"""

import socket
import time
import struct
import threading
from datetime import datetime
from typing import Optional, Tuple, Dict, Callable
import logging
from dataclasses import dataclass
from enum import Enum

# Import the command parser (assuming it's available)
try:
    from gimbalcmdparse import build_command
except ImportError:
    print("Warning: gimbalcmdparse module not found. Using embedded implementation.")
    
    def build_command(frame_header: str, address_bit1: str, address_bit2: str, 
                     control_bit: str, identifier_bit: str, data: str,
                     data_mode: str = 'ASCII', input_space_separate: bool = False,
                     output_format: str = 'ASCII', output_space_separate: bool = False) -> str:
        """Embedded basic command builder - replace with actual gimbalcmdparse when available"""
        # This is a simplified version - use the actual gimbalcmdparse module
        cmd = f"{frame_header}{address_bit1}{address_bit2}"
        if frame_header == '#tp':
            cmd += f"{len(data):X}"
        else:
            cmd += "2"
        cmd += f"{control_bit}{identifier_bit}{data}"
        # Add simple CRC (sum of bytes mod 256)
        crc = sum(cmd.encode()) & 0xFF
        cmd += f"{crc:02X}"
        return cmd

# Configuration (replace with your actual config)
GIMBAL_CONFIG = {
    'camera_ip': '192.168.0.108',  # Default from demo
    'control_port': 9003,
    'listen_port': 9004
}

class AngleMode(Enum):
    """Angle measurement modes"""
    MAGNETIC = "magnetic"  # Magnetic coding angle (relative to aircraft)
    GYRO = "gyro"         # Gyroscope angle (relative to spatial coordinates)

@dataclass
class GimbalAngles:
    """Container for gimbal angle data"""
    yaw: float      # degrees
    pitch: float    # degrees  
    roll: float     # degrees
    timestamp: datetime
    mode: AngleMode
    
    def __str__(self) -> str:
        return (f"Yaw: {self.yaw:7.2f}° | Pitch: {self.pitch:7.2f}° | "
                f"Roll: {self.roll:7.2f}° | Mode: {self.mode.value}")

class GimbalAngleMonitor:
    """
    Professional real-time gimbal angle monitoring system
    
    Features:
    - Real-time angle updates via UDP
    - Support for both magnetic and gyroscope modes  
    - Active sending mode for continuous updates
    - Robust error handling and reconnection
    - Thread-safe operations
    """
    
    def __init__(self, config: Dict = None):
        """Initialize the gimbal monitor with configuration"""
        config = config or GIMBAL_CONFIG
        
        self.camera_ip = config['camera_ip']
        self.control_port = config['control_port'] 
        self.listen_port = config['listen_port']
        
        # Communication sockets
        self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        self.recv_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        self.recv_sock.bind(('0.0.0.0', self.listen_port))
        self.recv_sock.settimeout(0.1)
        
        # State management
        self.running = False
        self.current_angles: Optional[GimbalAngles] = None
        self.angle_callbacks: list[Callable[[GimbalAngles], None]] = []
        self.active_sending_enabled = False
        self.current_mode = AngleMode.MAGNETIC
        
        # Threading
        self.receive_thread: Optional[threading.Thread] = None
        self.query_thread: Optional[threading.Thread] = None
        self.lock = threading.Lock()
        
        # Statistics
        self.stats = {
            'packets_received': 0,
            'parse_errors': 0,
            'last_update': None
        }
        
        # Setup logging
        logging.basicConfig(level=logging.INFO)
        self.logger = logging.getLogger(__name__)
        
    def add_angle_callback(self, callback: Callable[[GimbalAngles], None]):
        """Add callback function for angle updates"""
        self.angle_callbacks.append(callback)
        
    def _send_command(self, command: str) -> bool:
        """Send UDP command to gimbal"""
        try:
            self.sock.sendto(command.encode('utf-8'), (self.camera_ip, self.control_port))
            self.logger.debug(f"Sent command: {command}")
            return True
        except Exception as e:
            self.logger.error(f"Failed to send command {command}: {e}")
            return False
            
    def _parse_angle_response(self, data: str) -> Optional[GimbalAngles]:
        """
        Parse gimbal angle response according to protocol specification
        
        Format for GAC response: #tpUG C r GAC Y0Y1Y2Y3P0P1P2P3R0R1R2R3 CC
        Format for GIC response: #tpGU C r GICY0Y1Y2Y3P0P1P2P3R0R1R2R3 CC
        
        Angles are 4-character hex values representing signed 16-bit integers
        in 0.01 degree units, high byte first
        """
        try:
            data = data.strip()
            self.logger.debug(f"Parsing response: {data}")
            
            # Validate frame format
            if not (data.startswith('#tp') and len(data) >= 20):
                return None
                
            # Extract identifier to determine mode
            if 'GAC' in data:
                mode = AngleMode.MAGNETIC
                identifier = 'GAC'
            elif 'GIC' in data:
                mode = AngleMode.GYRO  
                identifier = 'GIC'
            else:
                return None
                
            # Find the angle data section (12 hex characters after identifier)
            id_pos = data.find(identifier)
            if id_pos == -1:
                return None
                
            angle_start = id_pos + 3  # Skip 3-char identifier
            angle_data = data[angle_start:angle_start + 12]
            
            if len(angle_data) != 12:
                self.logger.warning(f"Invalid angle data length: {len(angle_data)}")
                return None
                
            # Parse angles: Y0Y1Y2Y3 P0P1P2P3 R0R1R2R3
            yaw_hex = angle_data[0:4]
            pitch_hex = angle_data[4:8] 
            roll_hex = angle_data[8:12]
            
            # Convert hex to signed integers (0.01 degree units)
            yaw_raw = int(yaw_hex, 16)
            pitch_raw = int(pitch_hex, 16)
            roll_raw = int(roll_hex, 16)
            
            # Handle signed 16-bit conversion
            if yaw_raw > 32767:
                yaw_raw -= 65536
            if pitch_raw > 32767:
                pitch_raw -= 65536  
            if roll_raw > 32767:
                roll_raw -= 65536
                
            # Convert to degrees (protocol uses 0.01 degree units)
            yaw = yaw_raw / 100.0
            pitch = pitch_raw / 100.0
            roll = roll_raw / 100.0
            
            return GimbalAngles(
                yaw=yaw,
                pitch=pitch, 
                roll=roll,
                timestamp=datetime.now(),
                mode=mode
            )
            
        except Exception as e:
            self.logger.error(f"Failed to parse angle response '{data}': {e}")
            self.stats['parse_errors'] += 1
            return None
            
    def _receive_loop(self):
        """Background thread for receiving UDP responses"""
        self.logger.info("Starting receive loop")
        
        while self.running:
            try:
                data, addr = self.recv_sock.recvfrom(4096)
                message = data.decode('utf-8', errors='replace').strip()
                
                if not message:
                    continue
                    
                self.stats['packets_received'] += 1
                self.logger.debug(f"Received from {addr}: {message}")
                
                # Parse angle data
                angles = self._parse_angle_response(message)
                if angles:
                    with self.lock:
                        self.current_angles = angles
                        self.stats['last_update'] = datetime.now()
                        
                    # Notify callbacks
                    for callback in self.angle_callbacks:
                        try:
                            callback(angles)
                        except Exception as e:
                            self.logger.error(f"Callback error: {e}")
                            
            except socket.timeout:
                continue
            except Exception as e:
                if self.running:
                    self.logger.error(f"Receive loop error: {e}")
                    time.sleep(0.1)
                    
        self.logger.info("Receive loop stopped")
        
    def _query_loop(self):
        """Background thread for periodic angle queries when active sending is disabled"""
        self.logger.info("Starting query loop")
        
        while self.running:
            if not self.active_sending_enabled:
                self.query_angles()
                time.sleep(0.1)  # Query every 100ms
            else:
                time.sleep(1.0)  # Check less frequently when active sending is on
                
        self.logger.info("Query loop stopped")
        
    def enable_active_sending(self, mode: AngleMode = AngleMode.MAGNETIC) -> bool:
        """
        Enable active sending of gimbal angles
        
        Args:
            mode: AngleMode.MAGNETIC for magnetic coding, AngleMode.GYRO for gyroscope
        """
        if mode == AngleMode.MAGNETIC:
            # GAA command - Gimbal Attitude Active sending
            command = build_command(
                frame_header='#TP',
                address_bit1='P',  # Network source
                address_bit2='G',  # Gimbal destination
                control_bit='w',
                identifier_bit='GAA',
                data='01',  # Enable
                data_mode='ASCII'
            )
        else:
            # GIA command - Gyro attitude actively sending  
            command = build_command(
                frame_header='#TP',
                address_bit1='P',
                address_bit2='G', 
                control_bit='w',
                identifier_bit='GIA',
                data='01',  # Enable
                data_mode='ASCII'
            )
            
        success = self._send_command(command)
        if success:
            self.active_sending_enabled = True
            self.current_mode = mode
            self.logger.info(f"Active sending enabled for {mode.value} mode")
        return success
        
    def disable_active_sending(self) -> bool:
        """Disable active sending of gimbal angles"""
        # Send disable commands for both modes
        commands = [
            build_command('#TP', 'P', 'G', 'w', 'GAA', '00', 'ASCII'),  # Disable magnetic
            build_command('#TP', 'P', 'G', 'w', 'GIA', '00', 'ASCII')   # Disable gyro
        ]
        
        success = True
        for cmd in commands:
            success &= self._send_command(cmd)
            
        if success:
            self.active_sending_enabled = False
            self.logger.info("Active sending disabled")
        return success
        
    def query_angles(self, mode: AngleMode = AngleMode.MAGNETIC) -> bool:
        """
        Query gimbal angles once
        
        Args:
            mode: AngleMode.MAGNETIC for magnetic coding, AngleMode.GYRO for gyroscope
        """
        if mode == AngleMode.MAGNETIC:
            # GAC command - Read magnetic coding angle
            command = build_command(
                frame_header='#TP',
                address_bit1='P',
                address_bit2='G',
                control_bit='r', 
                identifier_bit='GAC',
                data='00',
                data_mode='ASCII'
            )
        else:
            # GIC command - Read gyroscope angle
            command = build_command(
                frame_header='#TP', 
                address_bit1='P',
                address_bit2='G',
                control_bit='r',
                identifier_bit='GIC', 
                data='00',
                data_mode='ASCII'
            )
            
        return self._send_command(command)
        
    def get_current_angles(self) -> Optional[GimbalAngles]:
        """Get the most recent angle measurement"""
        with self.lock:
            return self.current_angles
            
    def start(self, enable_active_sending: bool = True, mode: AngleMode = AngleMode.MAGNETIC):
        """
        Start the angle monitoring system
        
        Args:
            enable_active_sending: Whether to enable active sending for real-time updates
            mode: Angle measurement mode
        """
        if self.running:
            self.logger.warning("Monitor already running")
            return
            
        self.running = True
        self.logger.info(f"Starting gimbal angle monitor on {self.camera_ip}:{self.control_port}")
        
        # Start receive thread
        self.receive_thread = threading.Thread(target=self._receive_loop, daemon=True)
        self.receive_thread.start()
        
        # Start query thread  
        self.query_thread = threading.Thread(target=self._query_loop, daemon=True)
        self.query_thread.start()
        
        # Configure active sending
        if enable_active_sending:
            time.sleep(0.5)  # Allow threads to start
            self.enable_active_sending(mode)
        else:
            self.current_mode = mode
            
        self.logger.info("Gimbal angle monitor started successfully")
        
    def stop(self):
        """Stop the angle monitoring system"""
        if not self.running:
            return
            
        self.logger.info("Stopping gimbal angle monitor")
        
        # Disable active sending
        self.disable_active_sending()
        
        # Stop threads
        self.running = False
        
        if self.receive_thread:
            self.receive_thread.join(timeout=2.0)
        if self.query_thread:
            self.query_thread.join(timeout=2.0)
            
        # Close sockets
        try:
            self.sock.close()
            self.recv_sock.close()
        except:
            pass
            
        self.logger.info("Gimbal angle monitor stopped")
        
    def print_stats(self):
        """Print monitoring statistics"""
        print(f"\n=== Gimbal Monitor Statistics ===")
        print(f"Packets received: {self.stats['packets_received']}")
        print(f"Parse errors: {self.stats['parse_errors']}")
        print(f"Last update: {self.stats['last_update']}")
        print(f"Active sending: {self.active_sending_enabled}")
        print(f"Current mode: {self.current_mode.value}")
        print(f"==================================\n")

def print_angle_update(angles: GimbalAngles):
    """Callback function for printing angle updates"""
    timestamp = angles.timestamp.strftime("%H:%M:%S.%f")[:-3]
    print(f"[{timestamp}] {angles}")

def main():
    """Example usage of the GimbalAngleMonitor"""
    print("=== Professional Gimbal Angle Monitor ===")
    print("Real-time angle monitoring with SIP series protocol")
    print("Press Ctrl+C to stop\n")
    
    # Create monitor instance
    monitor = GimbalAngleMonitor(GIMBAL_CONFIG)
    
    # Add callback for real-time display
    monitor.add_angle_callback(print_angle_update)
    
    try:
        # Start monitoring with active sending
        monitor.start(enable_active_sending=True, mode=AngleMode.MAGNETIC)
        
        print("Monitoring started. Angle updates will appear below:")
        print("Format: [Time] Yaw: XXX.XX° | Pitch: XXX.XX° | Roll: XXX.XX° | Mode: magnetic/gyro\n")
        
        # Keep running and show stats periodically
        start_time = time.time()
        while True:
            time.sleep(10)  # Print stats every 10 seconds
            monitor.print_stats()
            
            # Example: Switch to gyro mode after 30 seconds
            if time.time() - start_time > 30 and monitor.current_mode == AngleMode.MAGNETIC:
                print("Switching to gyroscope mode...")
                monitor.enable_active_sending(AngleMode.GYRO)
                
    except KeyboardInterrupt:
        print("\nShutdown requested...")
    finally:
        monitor.stop()
        print("Monitor stopped. Goodbye!")

if __name__ == "__main__":
    main()