#!/usr/bin/env python3
"""
Eden Vision Tracker - Real face tracking with camera control
"""

import sys
sys.path.append('/Eden/CORE')

from eden_vision import EdenVision
from obsbot_control import ObsbotCamera
import time

class VisionTracker:
    """Combines vision with camera tracking"""
    
    def __init__(self):
        self.vision = EdenVision()
        self.camera = ObsbotCamera()
        
        self.tracking_enabled = True
        self.frame_width = 1280
        self.frame_height = 720
        
        print("✅ Vision tracker initialized")
    
    def calculate_pan_tilt(self, face_location):
        """Calculate pan/tilt needed to center face"""
        top, right, bottom, left = face_location
        
        # Calculate face center
        face_center_x = (left + right) / 2
        face_center_y = (top + bottom) / 2
        
        # Calculate offset from frame center
        frame_center_x = self.frame_width / 2
        frame_center_y = self.frame_height / 2
        
        offset_x = face_center_x - frame_center_x
        offset_y = face_center_y - frame_center_y
        
        # Convert to pan/tilt angles (simple proportional control)
        # Pan: -180 to 180 degrees
        # Tilt: -15 to 45 degrees
        
        pan_adjustment = (offset_x / frame_center_x) * 30  # Max 30 degree adjustment
        tilt_adjustment = -(offset_y / frame_center_y) * 15  # Max 15 degree adjustment (inverted)
        
        return pan_adjustment, tilt_adjustment
    
    def track_face(self):
        """Track a detected face"""
        # Get current observations
        obs = self.vision.perceive(include_text=False)
        
        if not obs.get('vision_available'):
            return False
        
        faces = obs.get('faces', [])
        
        if faces:
            # Track the first face (primary person)
            face = faces[0]
            location = face['location']
            name = face['name']
            
            # Calculate tracking adjustment
            pan_adj, tilt_adj = self.calculate_pan_tilt(location)
            
            # Only move if significant offset
            if abs(pan_adj) > 5 or abs(tilt_adj) > 3:
                print(f"📹 Tracking {name}: Pan {pan_adj:.1f}°, Tilt {tilt_adj:.1f}°")
                
                # Get current position
                current = self.camera.get_status()
                
                # Apply adjustment
                new_pan = current['pan'] + pan_adj
                new_tilt = current['tilt'] + tilt_adj
                
                # Clamp to limits
                new_pan = max(-180, min(180, new_pan))
                new_tilt = max(-15, min(45, new_tilt))
                
                # Move camera
                self.camera.pan(new_pan)
                self.camera.tilt(new_tilt)
                
                return True
            else:
                print(f"👁️ {name} centered")
                return True
        else:
            print("  No faces to track")
            return False
    
    def run_tracking_loop(self):
        """Main tracking loop"""
        print("\n🎯 Starting face tracking...")
        print("Press Ctrl+C to stop\n")
        
        try:
            while True:
                self.track_face()
                time.sleep(0.5)  # Track at 2Hz
                
        except KeyboardInterrupt:
            print("\n🛑 Stopping tracker...")
            self.camera.center()
            self.vision.release()

if __name__ == '__main__':
    tracker = VisionTracker()
    tracker.run_tracking_loop()