from dataclasses import dataclass
from typing import List, Tuple

@dataclass
class HandLandmark:
    """Single hand landmark point."""
    x: float
    y: float
    z: float
    confidence: float
    landmark_id: int

@dataclass
class HandGesture:
    """Detected hand gesture."""
    gesture_name: str
    confidence: float
    landmarks: List[HandLandmark]
    hand_side: str  # 'left' or 'right'

class HandGestureDetector:
    """Hand gesture detection from drone camera."""

    # Hand landmark connections
    HAND_CONNECTIONS = [
        (0, 1), (1, 2), (2, 3), (3, 4),  # Thumb
        (0, 5), (5, 6), (6, 7), (7, 8),  # Index
        (0, 9), (9, 10), (10, 11), (11, 12),  # Middle
        (0, 13), (13, 14), (14, 15), (15, 16),  # Ring
        (0, 17), (17, 18), (18, 19), (19, 20),  # Pinky
        (5, 9), (9, 13), (13, 17),  # Palm
    ]

    def __init__(self):
        self.gesture_templates = self._load_gesture_templates()

    def _load_gesture_templates(self):
        """Load reference gesture templates."""
        return {
            'open_palm': {
                'finger_angles': [160, 160, 160, 160, 160],  # All fingers extended
                'finger_lengths': [0.8, 0.9, 1.0, 0.9, 0.7],
            },
            'fist': {
                'finger_angles': [30, 30, 30, 30, 30],  # All fingers curled
                'finger_lengths': [0.3, 0.3, 0.3, 0.3, 0.3],
            },
            'pointing_up': {
                'finger_angles': [160, 30, 30, 30, 30],  # Only index extended
                'finger_lengths': [0.9, 0.3, 0.3, 0.3, 0.3],
            },
            'peace': {
                'finger_angles': [160, 160, 30, 30, 30],  # Index and middle
                'finger_lengths': [0.9, 0.9, 0.3, 0.3, 0.3],
            },
            'thumbs_up': {
                'finger_angles': [160, 30, 30, 30, 30],  # Thumb extended
                'finger_lengths': [0.9, 0.3, 0.3, 0.3, 0.3],
            },
        }

    def detect_landmarks(self, hand_region):
        """Detect hand landmarks (simplified)."""
        h, w = hand_region.shape[:2]
        landmarks = []

        # Simulated landmark positions (21 points)
        base_positions = [
            (0.5, 0.9),   # 0: Wrist
            (0.4, 0.75),  # 1: Thumb CMC
            (0.3, 0.6),   # 2: Thumb MCP
            (0.25, 0.45), # 3: Thumb IP
            (0.2, 0.3),   # 4: Thumb Tip
            (0.4, 0.5),   # 5: Index MCP
            (0.35, 0.35), # 6: Index PIP
            (0.3, 0.2),   # 7: Index DIP
            (0.25, 0.05), # 8: Index Tip
            (0.5, 0.45),  # 9: Middle MCP
            (0.5, 0.3),   # 10: Middle PIP
            (0.5, 0.15),  # 11: Middle DIP
            (0.5, 0.0),   # 12: Middle Tip
            (0.6, 0.5),   # 13: Ring MCP
            (0.65, 0.35), # 14: Ring PIP
            (0.7, 0.2),   # 15: Ring DIP
            (0.75, 0.1),  # 16: Ring Tip
            (0.7, 0.6),   # 17: Pinky MCP
            (0.75, 0.45), # 18: Pinky PIP
            (0.8, 0.3),   # 19: Pinky DIP
            (0.85, 0.2),  # 20: Pinky Tip
        ]

        for i, (nx, ny) in enumerate(base_positions):
            landmarks.append(HandLandmark(
                x=nx * w + np.random.uniform(-2, 2),
                y=ny * h + np.random.uniform(-2, 2),
                z=np.random.uniform(-0.1, 0.1),
                confidence=0.85 + np.random.uniform(-0.1, 0.1),
                landmark_id=i
            ))

        return landmarks

    def compute_finger_angles(self, landmarks):
        """Compute angles for each finger."""
        angles = []

        # Finger tip indices: 4, 8, 12, 16, 20
        # Finger pip indices: 3, 6, 10, 14, 18
        finger_tips = [4, 8, 12, 16, 20]
        finger_pips = [3, 6, 10, 14, 18]

        for tip_idx, pip_idx in zip(finger_tips, finger_pips):
            tip = landmarks[tip_idx]
            pip = landmarks[pip_idx]
            mcp = landmarks[pip_idx - 2]

            # Compute angle
            v1 = np.array([mcp.x - pip.x, mcp.y - pip.y])
            v2 = np.array([tip.x - pip.x, tip.y - pip.y])

            cos_angle = np.dot(v1, v2) / (np.linalg.norm(v1) * np.linalg.norm(v2) + 1e-6)
            angle = np.degrees(np.arccos(np.clip(cos_angle, -1, 1)))
            angles.append(angle)

        return angles

    def compute_finger_lengths(self, landmarks):
        """Compute relative finger lengths."""
        wrist = landmarks[0]
        tips = [4, 8, 12, 16, 20]

        lengths = []
        for tip_idx in tips:
            tip = landmarks[tip_idx]
            length = np.sqrt((tip.x - wrist.x)**2 + (tip.y - wrist.y)**2)
            lengths.append(length)

        # Normalize
        max_len = max(lengths) if lengths else 1
        return [l / max_len for l in lengths]

    def classify_gesture(self, landmarks):
        """Classify hand gesture from landmarks."""
        angles = self.compute_finger_angles(landmarks)
        lengths = self.compute_finger_lengths(landmarks)

        best_match = 'unknown'
        best_score = 0

        for gesture_name, template in self.gesture_templates.items():
            # Compare angles
            angle_diff = np.mean(np.abs(np.array(angles) - np.array(template['finger_angles'])))
            length_diff = np.mean(np.abs(np.array(lengths) - np.array(template['finger_lengths'])))

            # Combined similarity score
            score = 1.0 / (1.0 + angle_diff * 0.01 + length_diff)

            if score > best_score:
                best_score = score
                best_match = gesture_name

        return best_match, best_score

    def detect_gesture(self, hand_region):
        """Complete gesture detection pipeline."""
        # Detect landmarks
        landmarks = self.detect_landmarks(hand_region)

        # Classify gesture
        gesture_name, confidence = self.classify_gesture(landmarks)

        return HandGesture(
            gesture_name=gesture_name,
            confidence=confidence,
            landmarks=landmarks,
            hand_side='right'
        )

# Example: Hand gesture detection
np.random.seed(42)
detector = HandGestureDetector()

print("Hand Gesture Detection")
print("=" * 50)

# Simulate hand regions with different gestures
test_gestures = ['open_palm', 'fist', 'pointing_up', 'peace']

for gesture_type in test_gestures:
    hand_region = np.random.randint(0, 255, (200, 200, 3), dtype=np.uint8)
    result = detector.detect_gesture(hand_region)

    print(f"\nGesture: {gesture_type}")
    print(f"  Detected: {result.gesture_name}")
    print(f"  Confidence: {result.confidence:.1%}")
    print(f"  Landmarks: {len(result.landmarks)} points")

from enum import Enum
from dataclasses import dataclass
from typing import Optional

class DroneCommand(Enum):
    """Drone control commands."""
    HOVER = "hover"
    LAND = "land"
    TAKEOFF = "takeoff"
    ASCEND = "ascend"
    DESCEND = "descend"
    MOVE_FORWARD = "move_forward"
    MOVE_BACKWARD = "move_backward"
    MOVE_LEFT = "move_left"
    MOVE_RIGHT = "move_right"
    ROTATE_LEFT = "rotate_left"
    ROTATE_RIGHT = "rotate_right"
    RETURN_HOME = "return_home"
    EMERGENCY_STOP = "emergency_stop"
    FOLLOW = "follow"

@dataclass
class GestureCommand:
    """Mapping from gesture to drone command."""
    gesture: str
    command: DroneCommand
    description: str
    safety_level: str  # 'normal', 'caution', 'emergency'

class DroneGestureController:
    """Maps gestures to drone commands with safety validation."""

    def __init__(self):
        self.gesture_mappings = self._initialize_mappings()
        self.command_history = []
        self.gesture_buffer = []
        self.buffer_size = 5

        # Safety constraints
        self.min_confidence = 0.7
        self.required_consecutive = 3  # Required same gestures in a row
        self.emergency_cooldown = 5  # Seconds between emergency commands

    def _initialize_mappings(self):
        """Initialize gesture-to-command mappings."""
        return {
            'open_palm': GestureCommand(
                gesture='open_palm',
                command=DroneCommand.HOVER,
                description='Open palm signals drone to hover in place',
                safety_level='normal'
            ),
            'fist': GestureCommand(
                gesture='fist',
                command=DroneCommand.LAND,
                description='Closed fist signals drone to land',
                safety_level='caution'
            ),
            'pointing_up': GestureCommand(
                gesture='pointing_up',
                command=DroneCommand.ASCEND,
                description='Point up to make drone ascend',
                safety_level='normal'
            ),
            'pointing_down': GestureCommand(
                gesture='pointing_down',
                command=DroneCommand.DESCEND,
                description='Point down to make drone descend',
                safety_level='normal'
            ),
            'peace': GestureCommand(
                gesture='peace',
                command=DroneCommand.RETURN_HOME,
                description='Peace sign triggers return to home',
                safety_level='caution'
            ),
            'thumbs_up': GestureCommand(
                gesture='thumbs_up',
                command=DroneCommand.TAKEOFF,
                description='Thumbs up for takeoff',
                safety_level='normal'
            ),
            'swipe_left': GestureCommand(
                gesture='swipe_left',
                command=DroneCommand.MOVE_LEFT,
                description='Swipe left to move drone left',
                safety_level='normal'
            ),
            'swipe_right': GestureCommand(
                gesture='swipe_right',
                command=DroneCommand.MOVE_RIGHT,
                description='Swipe right to move drone right',
                safety_level='normal'
            ),
            'wave': GestureCommand(
                gesture='wave',
                command=DroneCommand.EMERGENCY_STOP,
                description='Wave gesture for emergency stop',
                safety_level='emergency'
            ),
        }

    def process_gesture(self, gesture_name, confidence, timestamp):
        """Process detected gesture and generate command."""
        # Validate confidence
        if confidence < self.min_confidence:
            return None, "Insufficient confidence"

        # Add to buffer
        self.gesture_buffer.append({
            'gesture': gesture_name,
            'confidence': confidence,
            'timestamp': timestamp
        })

        # Keep buffer size
        if len(self.gesture_buffer) > self.buffer_size:
            self.gesture_buffer.pop(0)

        # Check for gesture consistency
        if not self._check_consistency():
            return None, "Gesture not consistent"

        # Get command mapping
        if gesture_name not in self.gesture_mappings:
            return None, "Unknown gesture"

        mapping = self.gesture_mappings[gesture_name]

        # Safety validation
        is_safe, reason = self._validate_safety(mapping, timestamp)
        if not is_safe:
            return None, reason

        # Generate command
        self.command_history.append({
            'gesture': gesture_name,
            'command': mapping.command,
            'timestamp': timestamp,
            'confidence': confidence
        })

        return mapping.command, mapping.description

    def _check_consistency(self):
        """Check if recent gestures are consistent."""
        if len(self.gesture_buffer) < self.required_consecutive:
            return False

        recent = self.gesture_buffer[-self.required_consecutive:]
        gestures = [g['gesture'] for g in recent]

        return len(set(gestures)) == 1

    def _validate_safety(self, mapping, timestamp):
        """Validate command safety constraints."""
        # Check emergency cooldown
        if mapping.safety_level == 'emergency':
            for prev in reversed(self.command_history):
                if prev['command'] == DroneCommand.EMERGENCY_STOP:
                    time_diff = timestamp - prev['timestamp']
                    if time_diff < self.emergency_cooldown:
                        return False, "Emergency cooldown active"
                break

        # Check for conflicting commands
        if self.command_history:
            last_cmd = self.command_history[-1]['command']

            # Prevent rapid direction changes
            opposite_commands = {
                DroneCommand.MOVE_LEFT: DroneCommand.MOVE_RIGHT,
                DroneCommand.MOVE_RIGHT: DroneCommand.MOVE_LEFT,
                DroneCommand.ASCEND: DroneCommand.DESCEND,
                DroneCommand.DESCEND: DroneCommand.ASCEND,
            }

            if last_cmd in opposite_commands:
                if mapping.command == opposite_commands[last_cmd]:
                    return False, "Rapid direction change prevented"

        return True, "Command approved"

    def get_status(self):
        """Get controller status."""
        return {
            'buffer_size': len(self.gesture_buffer),
            'total_commands': len(self.command_history),
            'recent_commands': [
                {'gesture': c['gesture'], 'command': c['command'].value}
                for c in self.command_history[-5:]
            ]
        }

# Example: Gesture control system
np.random.seed(42)
controller = DroneGestureController()

print("Drone Gesture Control System")
print("=" * 50)

# Simulate gesture sequence
gestures_sequence = [
    ('thumbs_up', 0.9, 0),
    ('open_palm', 0.85, 1),
    ('open_palm', 0.88, 2),
    ('open_palm', 0.92, 3),  # Should trigger HOVER
    ('pointing_up', 0.87, 4),
    ('pointing_up', 0.91, 5),
    ('pointing_up', 0.89, 6),  # Should trigger ASCEND
    ('fist', 0.93, 7),
    ('fist', 0.95, 8),
    ('fist', 0.94, 9),  # Should trigger LAND
]

print("\nProcessing gesture sequence:")
for gesture, confidence, timestamp in gestures_sequence:
    command, message = controller.process_gesture(gesture, confidence, timestamp)

    if command:
        print(f"  [{timestamp}s] {gesture} → {command.value} ({message})")
    else:
        print(f"  [{timestamp}s] {gesture} → waiting ({message})")

# Show status
status = controller.get_status()
print(f"\nController Status:")
print(f"  Total commands: {status['total_commands']}")
print(f"  Recent commands: {[c['command'] for c in status['recent_commands']]}")

from collections import deque

class DynamicGestureRecognizer:
    """Recognize gestures that involve motion (swipes, waves, etc.)."""

    def __init__(self, sequence_length=15):
        self.sequence_length = sequence_length
        self.landmark_history = deque(maxlen=sequence_length)

    def update(self, landmarks):
        """Update with new landmark positions."""
        positions = [(lm.x, lm.y) for lm in landmarks]
        self.landmark_history.append(positions)

    def detect_swipe(self):
        """Detect swipe gestures."""
        if len(self.landmark_history) < 5:
            return None

        # Track index finger tip (landmark 8)
        positions = [hist[8] for hist in self.landmark_history if len(hist) > 8]
        if len(positions) < 5:
            return None

        # Compute horizontal movement
        x_positions = [p[0] for p in positions[-5:]]
        x_movement = x_positions[-1] - x_positions[0]

        # Threshold for swipe detection
        swipe_threshold = 0.15

        if x_movement > swipe_threshold:
            return 'swipe_right'
        elif x_movement < -swipe_threshold:
            return 'swipe_left'

        # Check vertical movement
        y_positions = [p[1] for p in positions[-5:]]
        y_movement = y_positions[-1] - y_positions[0]

        if y_movement > swipe_threshold:
            return 'swipe_down'
        elif y_movement < -swipe_threshold:
            return 'swipe_up'

        return None

    def detect_wave(self):
        """Detect waving gesture."""
        if len(self.landmark_history) < 8:
            return False

        # Track hand center movement
        centers = []
        for hist in self.landmark_history:
            if len(hist) > 0:
                x_mean = np.mean([p[0] for p in hist])
                y_mean = np.mean([p[1] for p in hist])
                centers.append((x_mean, y_mean))

        if len(centers) < 8:
            return False

        # Check for oscillatory motion
        x_positions = [c[0] for c in centers[-8:]]
        x_diffs = np.diff(x_positions)

        # Count direction changes
        direction_changes = 0
        for i in range(1, len(x_diffs)):
            if x_diffs[i] * x_diffs[i-1] < 0:
                direction_changes += 1

        # Wave requires multiple direction changes
        return direction_changes >= 3

    def detect_circle(self):
        """Detect circular motion gesture."""
        if len(self.landmark_history) < 10:
            return False

        # Track index finger tip
        positions = []
        for hist in self.landmark_history:
            if len(hist) > 8:
                positions.append(hist[8])

        if len(positions) < 10:
            return False

        # Check if points form a circle
        points = np.array(positions[-10:])
        center = np.mean(points, axis=0)
        distances = np.linalg.norm(points - center, axis=1)

        # Circle detection: consistent radius with angular progression
        radius_std = np.std(distances) / np.mean(distances)

        # Check angular progression
        angles = np.arctan2(points[:, 1] - center[1], points[:, 0] - center[0])
        angle_diffs = np.diff(angles)
        angle_wrap = (angle_diffs + np.pi) % (2 * np.pi) - np.pi
        angular_consistency = np.std(angle_wrap)

        return radius_std < 0.3 and angular_consistency < 0.5

    def recognize(self):
        """Recognize dynamic gesture from history."""
        # Check swipe
        swipe = self.detect_swipe()
        if swipe:
            return swipe, 0.85

        # Check wave
        if self.detect_wave():
            return 'wave', 0.80

        # Check circle
        if self.detect_circle():
            return 'circle', 0.75

        return None, 0.0

# Example: Dynamic gesture recognition
np.random.seed(42)
recognizer = DynamicGestureRecognizer()

print("Dynamic Gesture Recognition")
print("=" * 50)

# Simulate swipe gesture
print("\nSimulating swipe right gesture:")
for frame in range(10):
    # Create landmarks with rightward motion
    landmarks = []
    base_x = 0.3 + frame * 0.02  # Moving right
    for i in range(21):
        x = base_x + np.random.uniform(-0.02, 0.02)
        y = 0.5 + np.random.uniform(-0.02, 0.02)
        landmarks.append(HandLandmark(x=x, y=y, z=0, confidence=0.9, landmark_id=i))

    recognizer.update(landmarks)

    gesture, confidence = recognizer.recognize()
    if gesture:
        print(f"  Frame {frame+1}: Detected {gesture} ({confidence:.1%})")

# Simulate wave gesture
print("\nSimulating wave gesture:")
recognizer.landmark_history.clear()

for frame in range(12):
    # Oscillating horizontal motion
    x_offset = 0.1 * np.sin(frame * 1.5)
    landmarks = []
    for i in range(21):
        x = 0.5 + x_offset + np.random.uniform(-0.01, 0.01)
        y = 0.4 + np.random.uniform(-0.01, 0.01)
        landmarks.append(HandLandmark(x=x, y=y, z=0, confidence=0.9, landmark_id=i))

    recognizer.update(landmarks)

    gesture, confidence = recognizer.recognize()
    if gesture and frame >= 7:
        print(f"  Frame {frame+1}: Detected {gesture} ({confidence:.1%})")

from collections import deque
from datetime import datetime

class GestureDroneInterface:
    """Complete gesture control interface for drones."""

    def __init__(self):
        self.gesture_detector = HandGestureDetector()
        self.dynamic_recognizer = DynamicGestureRecognizer()
        self.command_controller = DroneGestureController()

        # Drone state
        self.drone_state = {
            'position': [0, 0, 10],  # x, y, z (altitude)
            'orientation': 0,  # yaw angle
            'battery': 100,
            'mode': 'standby',
            'commands_executed': 0,
        }

        # Safety monitoring
        self.safety_log = []
        self.gesture_history = deque(maxlen=50)

    def process_frame(self, frame, timestamp):
        """Process a camera frame for gesture control."""
        # Detect hand landmarks
        hand_region = frame  # Simplified
        gesture = self.gesture_detector.detect_gesture(hand_region)

        # Update dynamic recognizer
        self.dynamic_recognizer.update(gesture.landmarks)

        # Check for dynamic gesture
        dynamic_gesture, dyn_conf = self.dynamic_recognizer.recognize()

        # Use dynamic gesture if detected, otherwise static
        if dynamic_gesture and dyn_conf > 0.7:
            active_gesture = dynamic_gesture
            confidence = dyn_conf
        else:
            active_gesture = gesture.gesture_name
            confidence = gesture.confidence

        # Log gesture
        self.gesture_history.append({
            'gesture': active_gesture,
            'confidence': confidence,
            'timestamp': timestamp
        })

        # Process through command controller
        command, message = self.command_controller.process_gesture(
            active_gesture, confidence, timestamp
        )

        # Execute command if valid
        if command:
            self._execute_command(command)

        return {
            'gesture': active_gesture,
            'confidence': confidence,
            'command': command.value if command else None,
            'message': message,
            'drone_state': self.drone_state.copy()
        }

    def _execute_command(self, command):
        """Execute drone command."""
        # Update drone state based on command
        if command == DroneCommand.HOVER:
            self.drone_state['mode'] = 'hover'
        elif command == DroneCommand.LAND:
            self.drone_state['mode'] = 'landing'
            self.drone_state['position'][2] = max(0, self.drone_state['position'][2] - 2)
        elif command == DroneCommand.TAKEOFF:
            self.drone_state['mode'] = 'flying'
            self.drone_state['position'][2] = min(50, self.drone_state['position'][2] + 2)
        elif command == DroneCommand.ASCEND:
            self.drone_state['position'][2] = min(50, self.drone_state['position'][2] + 1)
        elif command == DroneCommand.DESCEND:
            self.drone_state['position'][2] = max(1, self.drone_state['position'][2] - 1)
        elif command == DroneCommand.MOVE_FORWARD:
            self.drone_state['position'][1] += 1
        elif command == DroneCommand.MOVE_LEFT:
            self.drone_state['position'][0] -= 1
        elif command == DroneCommand.MOVE_RIGHT:
            self.drone_state['position'][0] += 1
        elif command == DroneCommand.EMERGENCY_STOP:
            self.drone_state['mode'] = 'emergency'
            self.safety_log.append({
                'event': 'emergency_stop',
                'timestamp': datetime.now().isoformat()
            })
        elif command == DroneCommand.RETURN_HOME:
            self.drone_state['mode'] = 'returning'
            self.drone_state['position'] = [0, 0, self.drone_state['position'][2]]

        self.drone_state['commands_executed'] += 1

    def get_flight_log(self):
        """Get flight log summary."""
        return {
            'total_gestures': len(self.gesture_history),
            'total_commands': self.drone_state['commands_executed'],
            'unique_gestures': list(set(g['gesture'] for g in self.gesture_history)),
            'final_state': self.drone_state,
            'safety_events': len(self.safety_log)
        }

# Example: Complete gesture control session
np.random.seed(42)
interface = GestureDroneInterface()

print("Gesture-Controlled Drone Interface")
print("=" * 55)

# Simulate gesture commands
commands_sequence = [
    ('thumbs_up', 0.95),   # Takeoff
    ('open_palm', 0.90),   # Hover
    ('pointing_up', 0.88), # Ascend
    ('pointing_up', 0.92), # Ascend more
    ('swipe_right', 0.85), # Move right
    ('swipe_right', 0.87), # Move right more
    ('fist', 0.93),        # Land
]

print("\nProcessing gesture commands:")
for i, (gesture, confidence) in enumerate(commands_sequence):
    frame = np.random.randint(0, 255, (480, 640, 3), dtype=np.uint8)
    result = interface.process_frame(frame, timestamp=i)

    print(f"  [{i}s] Gesture: {gesture} ({confidence:.0%})")
    if result['command']:
        print(f"        Command: {result['command']}")
        state = result['drone_state']
        print(f"        Position: ({state['position'][0]}, "
              f"{state['position'][1]}, {state['position'][2]}m)")
        print(f"        Mode: {state['mode']}")

# Get flight log
log = interface.get_flight_log()
print(f"\nFlight Log Summary:")
print(f"  Total gestures detected: {log['total_gestures']}")
print(f"  Commands executed: {log['total_commands']}")
print(f"  Unique gestures used: {log['unique_gestures']}")
print(f"  Safety events: {log['safety_events']}")
print(f"  Final position: {log['final_state']['position']}")
print(f"  Final mode: {log['final_state']['mode']}")

print("\nGesture control session complete!")