[extern] camera auto-tracking integration guide with Rust example from coolstudio1678/norfair-rs #patch

[nmichlo]

Add comprehensive CAMERA_AUTOFOCUS_INTEGRATION.md with system architecture

• Include complete Rust implementation of AutoFocusController
• Add PTZ camera hardware integration with VISCA protocol support
• Include simple example: examples/camera_autofocus_basic.rs
• Cover configuration, debugging, performance metrics, and FAQs"

Translated files:

# Real-Time Camera Auto-Focus Integration Guide

**Real-Time Camera Auto-Focus Integration Guide for norfair-rs**

---

## 📖 Table of Contents

1. [Overview](#Overview)
2. [System Architecture](#System-Architecture)
3. [Core Concepts](#Core-Concepts)
4. [Complete Rust Implementation](#Complete-Rust-Implementation)
5. [Hardware Integration](#Hardware-Integration)
6. [Configuration and Debugging](#Configuration-and-Debugging)
7. [Performance Metrics](#Performance-Metrics)
8. [Common Questions](#Common-Questions)

---

## Overview

This guide shows how to integrate **norfair-rs** into real-time camera software, achieving:

✅ **Real-time Multi-object Tracking** - Track people/objects through stable IDs  
✅ **Auto Pan (Pan)** - Keep the subject in the center of the frame  
✅ **Auto Zoom (Zoom)** - Maintain constant object size  
✅ **Auto Focus (Focus)** - Adjust focal length according to distance  
✅ **Camera Motion Compensation** - Compensate for PTZ/handheld jitter  
✅ **Predictive Tracking** - Compensate for object motion in advance  
✅ **Performance Optimization** - 60-180 times faster than the original Python version  

### Performance Benchmarking

| Scenario | norfair (Python) | norfair-rs (Rust) | Speedup Ratio |
|------|-----------------|-------------------|-------|
| Small Scale | 4,700 fps | 296,000 fps | **63x** |
| Medium | 540 fps | 89,000 fps | **165x** |
| Large Scale | 101 fps | 41,000 fps | **406x** |

---

## System Architecture

### Data Flow

```
┌────────────────────────────────────────────────────────────────────────┐
│                        Camera Input                                │
│                    (30/60 FPS Video Stream)                            │
└────────────────────────────────┬────────────────────────────────────────┘
                         │
        ┌────────────────────────────────────────┐
        │   Object Detection (YOLO/Faster R-CNN)   │
        │  Detect bounding boxes and confidence of people/objects   │
        └────────────┬─────────────────────────┘
                     │ detections[]
        ┌────────────────────────────────────────┐
        │    Tracker (norfair-rs)           │
        │  • Allocate/maintain stable IDs               │
        │  • Compensate for camera motion                  │
        │  • Velocity estimation and prediction                │
        │  • Kalman filter state maintenance            │
        └────────────┬─────────────────────────┘
                     │ tracked_objects[]
        ┌────────────────────────────────────────┐
        │  Auto Focus Controller                  │
        │  (AutoFocusController)          │
        │  • PID pan control                  │
        │  • Zoom feedback control                  │
        │  • Depth estimation focus value                │
        │  • Predictive motion compensation                │
        └────────────┬─────────────────────────┘
                     │ FocusCommand
        ┌────────────────────────────────────────┐
        │  Camera Hardware Control Interface                │
        │  (CameraHardware trait)         │
        └────────────┬─────────────────────────┘
                     │
        ┌────────────────────────────────────────┐
        │  Camera Hardware                        │
        │  • PTZ Gimbal (VISCA Protocol)          │
        │  • Zoom Lens (USB/Serial)        │
        │  • Auto Focus Motor                  │
        └────────────────────────────────────────┘
```

### Timing

```
[Camera Frame] → [YOLO Detection] → [norfair Tracking] → [Focus Calculation] → [Hardware Command]
  0ms      50-100ms      5-10ms          2-5ms        1-2ms
  └─────────────────────────────────────────────────────────────┘
                    Total Latency: 60-120ms (sufficient for real-time tracking)
```

---

## Core Concepts

### 1. Relative Coordinates vs Absolute Coordinates

```rust
// Relative coordinates (Relative) = Camera frame coordinate system
// • (0,0) = Top left corner
// • (w,h) = Bottom right corner  
// • Changes with camera motion
// • Used for distance calculation, display

// Absolute coordinates (Absolute) = World coordinate system
// • (0,0) = Top left corner of the first frame
// • Fixed and unchanged (after compensating for camera motion)
// • Used for Kalman filter internal state
// • Used for real-world position tracking

let relative = obj.estimate;           // Frame coordinates
let absolute = obj.get_estimate(true); // World coordinates
```

### 2. Tracking Lifecycle

```rust
TrackedObject state transition:

Create (Create)
  ↓
Initialization Phase (Initialization Phase)
  ├─ is_initializing = true
  ├─ id = None (No permanent ID)
  ├─ initializing_id = Some(N) (Temporary ID)
  ├─ Requires initialization_delay detections
  └─ Dies if exceeding hit_counter
  ↓
Active Phase (Active Phase)
  ├─ is_initializing = false
  ├─ id = Some(N) (Permanent ID)
  ├─ Participates in tracking and matching
  └─ Decays frame by frame when no match
  ↓
Decay Phase (Decay Phase)
  ├─ hit_counter > 0 (Still visible)
  ├─ hit_counter -= 1 when no new detection
  └─ Used to handle temporary occlusion
  ↓
Death/Re-identification (Death/ReID)
  ├─ hit_counter < 0
  ├─ If ReID is enabled, try matching past_detections
  └─ If failed, completely remove
```

### 3. Auto Focus Control

```rust
// PID pan control
Pan control:  Error = Object center X - Frame center X
          Output = Error × 0.02 (P coefficient)
          Range = [-1.0, +1.0] (Left to right)

Tilt control: Error = Object center Y - Frame center Y
          Output = Error × 0.02 (P coefficient)
          Range = [-1.0, +1.0] (Bottom to top)

// Zoom feedback control
Target size = Frame width × 30%
Current size = (Object width + Object height) / 2
Scale factor = Target size / Current size
Output = (Scale factor - 1.0) × 0.05 (Feedback gain)
Range = [-0.5, +0.5] (Zoom out to zoom in)

// Depth estimation focus
Estimated distance = (Frame width × Frame height) / (Object width × Object height)
Focus value = (Distance / 1000.0).min(1.0) × 1000
Range = [0, 1000] (Focus units)

// Predictive tracking
Prediction time = 100ms (Position after 100 milliseconds)
Predicted X = Current X + Velocity X × Prediction time
Predicted Y = Current Y + Velocity Y × Prediction time
Predicted pan = (Predicted Y - Frame center Y) × 0.02
```

---

## Complete Rust Implementation

### Key Data Structures

```rust
use norfair_rs::{Detection, Tracker, TrackerConfig};
use norfair_rs::camera_motion::TranslationTransformation;
use nalgebra::DMatrix;
use std::time::Instant;

/// Subject information being tracked
pub struct SubjectTracker {
    /// Current tracked object ID
    pub primary_id: Option<i32>,
    
    /// Object position in the world coordinate system (Absolute coordinates)
    pub world_position: (f64, f64),
    
    /// Object position in the camera frame (Relative coordinates)
    pub frame_position: (f64, f64),
    
    /// Object velocity vector [vx, vy]
    pub velocity: (f64, f64),
    
    /// Object width and height
    pub size: (f64, f64),
    
    /// Tracking confidence (0.0 = newly created, 1.0 = fully stable)
    pub confidence: f64,
    
    /// Last update time
    pub last_update: Instant,
}

impl SubjectTracker {
    pub fn new() -> Self {
        Self {
            primary_id: None,
            world_position: (0.0, 0.0),
            frame_position: (0.0, 0.0),
            velocity: (0.0, 0.0),
            size: (0.0, 0.0),
            confidence: 0.0,
            last_update: Instant::now(),
        }
    }
}

/// Focus control command
#[derive(Debug, Clone)]
pub struct FocusCommand {
    /// Pan speed (-1 = left, +1 = right)
    pub pan: f64,
    
    /// Tilt speed (-1 = down, +1 = up)
    pub tilt: f64,
    
    /// Zoom speed (-0.5 = zoom out, +0.5 = zoom in)
    pub zoom_speed: f64,
    
    /// Focus value (0-1000)
    pub focus_value: u16,
    
    /// Tracked object ID
    pub tracking_id: Option<i32>,
    
    /// Tracking confidence (0.0-1.0)
    pub confidence: f64,
    
    /// Subject position in the frame (for UI display)
    pub subject_position: (f64, f64),
    
    /// Subject size (for UI display)
    pub subject_size: (f64, f64),
}

impl Default for FocusCommand {
    fn default() -> Self {
        Self {
            pan: 0.0,
            tilt: 0.0,
            zoom_speed: 0.0,
            focus_value: 500,
            tracking_id: None,
            confidence: 0.0,
            subject_position: (0.0, 0.0),
            subject_size: (0.0, 0.0),
        }
    }
}
```

### Auto Focus Controller

```rust
/// Auto Focus Controller - Core Logic
pub struct AutoFocusController {
    /// norfair tracker
    tracker: Tracker,
    
    /// Current primary tracked object
    subject: SubjectTracker,
    
    /// Frame size
    frame_width: f64,
    frame_height: f64,
}

impl AutoFocusController {
    /// Create a new auto focus controller
    pub fn new(frame_width: f64, frame_height: f64) 
        -> Result<Self, Box<dyn std::error::Error>> 
    {
        // Configure tracker: optimized for people/object tracking
        let mut config = TrackerConfig::from_distance_name("iou", 0.5);
        config.hit_counter_max = 30;           // Keep tracking for 30 frames (when no detection)
        config.initialization_delay = 3;       // Requires 3 detections to allocate ID
        config.detection_threshold = 0.5;      // Minimum detection confidence
        config.past_detections_length = 10;    // Store history for velocity estimation
        
        let tracker = Tracker::new(config)?;
        
        Ok(Self {
            tracker,
            subject: SubjectTracker::new(),
            frame_width,
            frame_height,
        })
    }

    /// Update tracking + calculate auto focus parameters
    pub fn update(
        &mut self,
        detections: Vec<Detection>,
        optical_flow: Option<(f64, f64)>,  // Camera motion: (dx, dy)
    ) -> FocusCommand 
    {
        // ========== Step 1: Compensate for camera motion ==========
        let coord_transform = optical_flow.map(|(dx, dy)| {
            TranslationTransformation::new([dx, dy])
        });

        // ========== Step 2: Update tracker ==========
        let tracked_objects = self.tracker.update(
            detections,
            1,  // period = 1 frame
            coord_transform.as_ref()
                .map(|t| t as &dyn norfair_rs::camera_motion::CoordinateTransformation),
        );

        // ========== Step 3: Select primary tracked object ==========
        // Strategy: Select the largest and most stable object
        let best_object = tracked_objects
            .iter()
            .filter(|obj| obj.id.is_some() && !obj.is_initializing)
            .max_by(|a, b| {
                // Compare object area
                let size_a = (a.estimate[(0, 2)] - a.estimate[(0, 0)])
                           * (a.estimate[(1, 3)] - a.estimate[(1, 1)]);
                let size_b = (b.estimate[(0, 2)] - b.estimate[(0, 0)])
                           * (b.estimate[(1, 3)] - b.estimate[(1, 1)]);
                size_a.partial_cmp(&size_b).unwrap_or(std::cmp::Ordering::Equal)
            });

        if let Some(obj) = best_object {
            self.update_subject_from_object(obj, coord_transform.as_ref());
        }

        // ========== Step 4: Calculate focus control command ==========
        self.compute_focus_command()
    }

    /// Update subject information from tracked object
    fn update_subject_from_object(
        &mut self,
        obj: &norfair_rs::TrackedObject,
        _transform: Option<&TranslationTransformation>,
    ) {
        self.subject.primary_id = obj.id;
        
        // Extract bounding box coordinates
        let bbox = &obj.estimate;
        let x1 = bbox[(0, 0)];
        let y1 = bbox[(0, 1)];
        let x2 = bbox[(0, 2)];
        let y2 = bbox[(0, 3)];
        
        // Camera frame coordinates
        let frame_cx = (x1 + x2) / 2.0;
        let frame_cy = (y1 + y2) / 2.0;
        self.subject.frame_position = (frame_cx, frame_cy);
        
        // Object size
        let width = (x2 - x1).abs();
        let height = (y2 - y1).abs();
        self.subject.size = (width, height);
        
        // Velocity estimation (based on past detection history)
        if obj.past_detections.len() >= 2 {
            if let (Some(latest), Some(oldest)) = 
                (obj.past_detections.back(), obj.past_detections.front()) 
            {
                let dt = (obj.age as f64) / 30.0;  // Assume 30fps
                if dt > 0.0 {
                    self.subject.velocity = (
                        (latest.points[(0, 0)] - oldest.points[(0, 0)]) / dt,
                        (latest.points[(0, 1)] - oldest.points[(0, 1)]) / dt,
                    );
                }
            }
        }
        
        // Confidence (based on hit_counter and age)
        self.subject.confidence = (obj.hit_counter as f64).min(obj.age as f64) / 30.0;
        self.subject.last_update = Instant::now();
    }

    /// Calculate focus control command
    fn compute_focus_command(&self) -> FocusCommand {
        // If there is no tracked object, return empty command
        if self.subject.primary_id.is_none() {
            return FocusCommand::default();
        }

        let (cx, cy) = self.subject.frame_position;
        let (width, height) = self.subject.size;
        let (vx, vy) = self.subject.velocity;

        // ========== Control 1: Pan/Tilt ==========
        // Target: Keep subject in frame center
        // Method: PID control
        
        let center_x = self.frame_width / 2.0;
        let center_y = self.frame_height / 2.0;
        
        let pan_error = cx - center_x;      // Horizontal deviation
        let tilt_error = cy - center_y;     // Vertical deviation
        
        let pan_speed = (pan_error * 0.02).clamp(-1.0, 1.0);   // P=0.02
        let tilt_speed = (tilt_error * 0.02).clamp(-1.0, 1.0);

        // ========== Control 2: Zoom ==========
        // Target: Maintain constant object size
        // Method: Feedback control
        
        let target_size = self.frame_width * 0.3;  // Target: 30% of frame width
        let current_size = (width + height) / 2.0;  // Current average size
        
        let zoom_factor = if current_size > 0.0 {
            target_size / current_size
        } else {
            1.0
        };
        
        // Limit zoom speed to avoid jitter
        let zoom_speed = ((zoom_factor - 1.0) * 0.05).clamp(-0.5, 0.5);

        // ========== Control 3: Focus ==========
        // Target: Adjust focus value according to object distance
        // Method: Estimate depth based on bounding box size
        
        // Rough estimation: Assume fixed size object
        // Distance ∝ (Total pixel area) / (Object pixel area)
        let estimated_distance = (self.frame_width * self.frame_height) 
            / (width * height).max(1.0);
        
        // Map to focus value (0-1000)
        let focus_value = ((estimated_distance / 1000.0).min(1.0) * 1000.0) as u16;

        // ========== Control 4: Predictive Tracking ==========
        // Target: Compensate object motion in advance
        // Method: Predict position after 100ms
        
        let prediction_time = 0.1;  // seconds
        let predicted_x = cx + vx * prediction_time;
        let predicted_y = cy + vy * prediction_time;
        
        // Predicted pan correction
        let predict_pan = (predicted_x - center_x) * 0.02;
        let predict_tilt = (predicted_y - center_y) * 0.02;
        
        // Combine base control and predictive control
        let final_pan = (pan_speed + predict_pan * 0.3).clamp(-1.0, 1.0);
        let final_tilt = (tilt_speed + predict_tilt * 0.3).clamp(-1.0, 1.0);

        FocusCommand {
            pan: final_pan,
            tilt: final_tilt,
            zoom_speed,
            focus_value,
            tracking_id: self.subject.primary_id,
            confidence: self.subject.confidence,
            subject_position: self.subject.frame_position,
            subject_size: self.subject.size,
        }
    }
}
```

---

## Hardware Integration

### Hardware Control Interface

```rust
/// Hardware abstraction interface
pub trait CameraHardware {
    fn apply_focus_command(&mut self, cmd: &FocusCommand) 
        -> Result<(), Box<dyn std::error::Error>>;
}

/// PTZ Camera Implementation (VISCA Protocol)
/// Support: Sony EVI, Panasonic and other manufacturers
pub struct PTZCamera {
    serial_port: Box<dyn serialport::SerialPort>,
    current_zoom: f64,
    current_focus: u16,
}

impl PTZCamera {
    pub fn new(port_name: &str) 
        -> Result<Self, Box<dyn std::error::Error>> 
    {
        let settings = serialport::SerialPortSettings {
            baud_rate: 9600,
            data_bits: serialport::DataBits::Eight,
            flow_control: serialport::FlowControl::None,
            parity: serialport::Parity::None,
            stop_bits: serialport::StopBits::One,
            timeout: std::time::Duration::from_millis(100),
        };
        
        let port = serialport::open_with_settings(port_name, &settings)?;
        
        Ok(Self {
            serial_port: Box::new(port),
            current_zoom: 1.0,
            current_focus: 500,
        })
    }
}

impl CameraHardware for PTZCamera {
    fn apply_focus_command(&mut self, cmd: &FocusCommand) 
        -> Result<(), Box<dyn std::error::Error>> 
    {
        // Pan/Tilt - Only send on significant change
        if cmd.pan.abs() > 0.01 || cmd.tilt.abs() > 0.01 {
            self.send_pan_tilt_command(cmd.pan, cmd.tilt)?;
        }

        // Zoom - Cumulative zoom
        if cmd.zoom_speed.abs() > 0.01 {
            self.current_zoom = (self.current_zoom + cmd.zoom_speed)
                .clamp(1.0, 10.0);
            self.send_zoom_command(self.current_zoom)?;
        }

        // Focus - Only send when difference exceeds threshold
        if (cmd.focus_value as i32 - self.current_focus as i32).abs() > 10 {
            self.current_focus = cmd.focus_value;
            self.send_focus_command(cmd.focus_value)?;
        }

        Ok(())
    }
}

// VISCA command implementation
impl PTZCamera {
    fn send_pan_tilt_command(&mut self, pan: f64, tilt: f64) 
        -> Result<(), Box<dyn std::error::Error>> 
    {
        // Speed: -1.0 (leftmost) to +1.0 (rightmost)
        let pan_speed = ((pan.abs() * 24.0) as u8).clamp(1, 24);
        let tilt_speed = ((tilt.abs() * 20.0) as u8).clamp(1, 20);
        
        // VISCA format: $81 $01 $06 $01 [PanSpeed] [TiltSpeed] $FF
        let cmd = vec![0x81, 0x01, 0x06, 0x01, pan_speed, tilt_speed, 0xFF];
        self.serial_port.write_all(&cmd)?;
        
        Ok(())
    }

    fn send_zoom_command(&mut self, zoom: f64) 
        -> Result<(), Box<dyn std::error::Error>> 
    {
        // Zoom: 1.0 (no zoom) to 10.0 (max zoom)
        // Map to 12-bit value: 0 (none) to 4095 (max)
        let zoom_value = ((zoom - 1.0) * 4000.0 / 9.0) as u16;
        let high_byte = ((zoom_value >> 8) & 0xFF) as u8;
        let low_byte = (zoom_value & 0xFF) as u8;
        
        // VISCA format: $81 $01 $04 $47 [H] [L] $FF
        let cmd = vec![0x81, 0x01, 0x04, 0x47, high_byte, low_byte, 0xFF];
        self.serial_port.write_all(&cmd)?;
        
        Ok(())
    }

    fn send_focus_command(&mut self, focus: u16) 
        -> Result<(), Box<dyn std::error::Error>> 
    {
        // Focus: 0 (infinity) to 1000 (closest)
        // Map to 12-bit value: 0 to 4095
        let focus_value = (focus as f64 * 4095.0 / 1000.0) as u16;
        let high_byte = ((focus_value >> 8) & 0xFF) as u8;
        let low_byte = (focus_value & 0xFF) as u8;
        
        // VISCA format: $81 $01 $04 $48 [H] [L] $FF
        let cmd = vec![0x81, 0x01, 0x04, 0x48, high_byte, low_byte, 0xFF];
        self.serial_port.write_all(&cmd)?;
        
        Ok(())
    }
}
```

---

## Configuration and Debugging

### Recommended Parameters

```rust
// Tracker configuration
config.hit_counter_max = 30;              // Keep active frames (20-50)
config.initialization_delay = 3;          // Initialization delay (2-5)
config.distance_threshold = 0.5;          // IoU threshold (0.3-0.7)
config.detection_threshold = 0.5;         // Detection confidence (0.4-0.7)
config.past_detections_length = 10;       // Historical detection count (5-15)

// Control parameters
const PAN_P: f64 = 0.02;                  // Pan P coefficient (0.01-0.05)
const TILT_P: f64 = 0.02;                 // Tilt P coefficient (0.01-0.05)
const ZOOM_GAIN: f64 = 0.05;              // Zoom gain (0.02-0.1)
const PREDICTION_TIME: f64 = 0.1;         // Prediction time (50-200ms)
const TARGET_SIZE_RATIO: f64 = 0.3;       // Target size ratio (0.2-0.5)
```

### Debug Output

```rust
fn print_debug_info(cmd: &FocusCommand) {
    println!("=== Focus Command ===");
    println!("ID: {:?}", cmd.tracking_id);
    println!("Confidence: {:.2}%", cmd.confidence * 100.0);
    println!("Position: ({:.0}, {:.0})", 
        cmd.subject_position.0, cmd.subject_position.1);
    println!("Size: {:.0} x {:.0}", 
        cmd.subject_size.0, cmd.subject_size.1);
    println!("Pan: Pan={:+.3}, Tilt={:+.3}", cmd.pan, cmd.tilt);
    println!("Zoom: {:.3}x", 1.0 + cmd.zoom_speed);
    println!("Focus: {}", cmd.focus_value);
}
```

---

## Performance Metrics

| Metric | Value | Description |
|------|-----|------|
| Detection Latency | 50-100ms | YOLO inference (depends on model size) |
| Tracking Latency | 5-10ms | norfair update |
| Control Calculation | 2-5ms | PID + prediction |
| Hardware Communication | 1-2ms | Serial port send |
| **Total Latency** | **60-120ms** | ~6-12 frames @60fps |
| **Pure Tracking Throughput** | **1000+ fps** | norfair-rs tracking only |

### Optimization Suggestions

1. **Use lightweight detection models** (nano/small instead of large)
2. **Adjust image resolution** (640x480 instead of 1920x1080)
3. **Enable GPU acceleration** (CUDA/CoreML)
4. **Asynchronous processing** Put detection and tracking on different threads
5. **Reduce frame rate** (30fps instead of 60fps)
6. **Reduce history length** `past_detections_length = 5`

---

## Common Questions

### Q1: What if tracking IDs change frequently?

**A:** Increase `hit_counter_max` and `initialization_delay`
```rust
config.hit_counter_max = 50;           // Increase from 30 to 50
config.initialization_delay = 5;       // Increase from 3 to 5
```

### Q2: Tracking latency too high?

**A:** Check the following points:
- Is YOLO latency too high (switch to nano model)
- Is GPU enabled (should be enabled)
- Is image resolution too high (switch to 640x480)

### Q3: Auto focus oscillation/jitter?

**A:** Reduce PID gain or add dead zone
```rust
const PAN_P: f64 = 0.01;               // Reduce from 0.02 to 0.01
const DEAD_ZONE: f64 = 5.0;            // Respond only when deviation > 5 pixels
```

### Q4: Cannot connect to camera hardware?

**A:** Check serial port settings
```bash
# Linux
ls /dev/ttyUSB*
screen /dev/ttyUSB0 9600

# Windows  
mode COM1 BAUD=9600 PARITY=N
```

### Q5: How to select tracking target with multiple objects?

**A:** Current implementation selects the largest object. Can be changed to:
```rust
// Select the closest to the last position
let best = tracked_objects
    .iter()
    .min_by_key(|obj| {
        let dist = (obj.estimate[(0, 0)] - last_x).powi(2)
                 + (obj.estimate[(0, 1)] - last_y).powi(2);
        dist as i32
    });
```

---

## License

BSD 3-Clause License (Same as norfair-rs)

## Reference Resources

- [norfair-rs GitHub](https://github.com/nmichlo/norfair-rs)
- [norfair original project](https://github.com/tryolabs/norfair)
- [VISCA protocol documentation](https://en.wikipedia.org/wiki/VISCA)
- [OpenCV optical flow tutorial](https://docs.opencv.org/master/d7/d8b/tutorial_py_lucas_kanade.html)

---

**Last Updated**: 2024  
**Maintainer**: norfair community  
**Contributors**: PRs welcome

And the next one:

//! Simple camera auto focus example
//!
//! This example shows how to use norfair-rs to implement basic auto focus functionality.
//! Does not include hardware integration, only demonstrates tracking and focus calculation logic.
//!
//! Usage: 
//!   cargo run --example camera_autofocus_basic

use norfair_rs::{Detection, Tracker, TrackerConfig};

/// Simplified focus controller
pub struct SimpleFocusController {
    tracker: Tracker,
    frame_width: f64,
    frame_height: f64,
}

/// Focus command
#[derive(Debug, Clone)]
pub struct FocusCommand {
    pub pan: f64,           // [-1, 1]
    pub tilt: f64,          // [-1, 1]
    pub zoom_speed: f64,    // [-0.5, 0.5]
    pub focus_value: u16,   // [0, 1000]
    pub tracking_id: Option<i32>,
    pub confidence: f64,
}

impl SimpleFocusController {
    pub fn new(frame_width: f64, frame_height: f64) 
        -> Result<Self, Box<dyn std::error::Error>> 
    {
        let mut config = TrackerConfig::from_distance_name("iou", 0.5);
        config.hit_counter_max = 30;
        config.initialization_delay = 3;
        
        let tracker = Tracker::new(config)?;
        
        Ok(Self {
            tracker,
            frame_width,
            frame_height,
        })
    }
    
    pub fn update(&mut self, detections: Vec<Detection>) -> FocusCommand {
        let tracked_objects = self.tracker.update(detections, 1, None);
        
        // Find the largest active object
        let best = tracked_objects
            .iter()
            .filter(|obj| obj.id.is_some())
            .max_by(|a, b| {
                let size_a = (a.estimate[(0, 2)] - a.estimate[(0, 0)])
                           * (a.estimate[(1, 3)] - a.estimate[(1, 1)]);
                let size_b = (b.estimate[(0, 2)] - b.estimate[(0, 0)])
                           * (b.estimate[(1, 3)] - b.estimate[(1, 1)]);
                size_a.partial_cmp(&size_b).unwrap_or(std::cmp::Ordering::Equal)
            });
        
        if let Some(obj) = best {
            let bbox = &obj.estimate;
            let cx = (bbox[(0, 0)] + bbox[(0, 2)]) / 2.0;
            let cy = (bbox[(0, 1)] + bbox[(0, 3)]) / 2.0;
            let w = (bbox[(0, 2)] - bbox[(0, 0)]).abs();
            let h = (bbox[(1, 3)] - bbox[(1, 1)]).abs();
            
            // Pan
            let center_x = self.frame_width / 2.0;
            let center_y = self.frame_height / 2.0;
            let pan = ((cx - center_x) * 0.02).clamp(-1.0, 1.0);
            let tilt = ((cy - center_y) * 0.02).clamp(-1.0, 1.0);
            
            // Zoom
            let target_size = self.frame_width * 0.3;
            let current_size = (w + h) / 2.0;
            let zoom_factor = if current_size > 0.0 {
                target_size / current_size
            } else {
                1.0
            };
            let zoom_speed = ((zoom_factor - 1.0) * 0.05).clamp(-0.5, 0.5);
            
            // Focus
            let distance = (self.frame_width * self.frame_height) / (w * h).max(1.0);
            let focus_value = ((distance / 1000.0).min(1.0) * 1000.0) as u16;
            
            FocusCommand {
                pan,
                tilt,
                zoom_speed,
                focus_value,
                tracking_id: obj.id,
                confidence: (obj.hit_counter as f64 / 30.0).min(1.0),
            }
        } else {
            FocusCommand {
                pan: 0.0,
                tilt: 0.0,
                zoom_speed: 0.0,
                focus_value: 500,
                tracking_id: None,
                confidence: 0.0,
            }
        }
    }
}

fn main() -> Result<(), Box<dyn std::error::Error>> {
    println!("norfair-rs Auto Focus Demo");
    println!("========================\n");
    
    // Create controller (assuming 640x480 resolution)
    let mut controller = SimpleFocusController::new(640.0, 480.0)?;
    
    // Simulate 3 frames of detection data
    let frames = vec![
        // Frame 1: One detection
        vec![
            Detection::from_slice(&[100.0, 150.0, 200.0, 300.0], 1, 4)?,
        ],
        // Frame 2: Same object, slightly changed position
        vec![
            Detection::from_slice(&[110.0, 155.0, 210.0, 305.0], 1, 4)?,
        ],
        // Frame 3: Continue tracking
        vec![
            Detection::from_slice(&[120.0, 160.0, 220.0, 310.0], 1, 4)?,
        ],
    ];
    
    for (frame_idx, detections) in frames.iter().enumerate() {
        let cmd = controller.update(detections.clone());
        
        println!("Frame {}:", frame_idx);
        println!("  ID: {:?}", cmd.tracking_id);
        println!("  Confidence: {:.2}%", cmd.confidence * 100.0);
        println!("  Pan: Pan={:+.3}, Tilt={:+.3}", cmd.pan, cmd.tilt);
        println!("  Zoom speed: {:.3}", cmd.zoom_speed);
        println!("  Focus value: {}", cmd.focus_value);
        println!();
    }
    
    Ok(())
}