mecha10_controllers/mock/

camera.rs

//! Mock camera controller for testing

use crate::camera::*;
use crate::{Controller, ControllerCapabilities, ControllerError, ControllerHealth, ControllerState};
use async_trait::async_trait;
use mecha10_core::sensor::{CameraInfo, DepthImage, RgbImage};
use std::time::{SystemTime, UNIX_EPOCH};

/// Mock camera controller configuration
#[derive(Debug, Clone)]
pub struct MockCameraConfig {
    pub width: u32,
    pub height: u32,
    pub fps: u32,
    pub has_depth: bool,
    pub has_infrared: bool,
}

impl Default for MockCameraConfig {
    fn default() -> Self {
        Self {
            width: 640,
            height: 480,
            fps: 30,
            has_depth: true,
            has_infrared: false,
        }
    }
}

/// Mock camera controller for testing
///
/// Generates synthetic frames with configurable patterns.
pub struct MockCameraController {
    config: MockCameraConfig,
    state: ControllerState,
    frame_count: u64,
    intrinsics: CameraIntrinsics,
    parameters: CameraParameters,
}

#[async_trait]
impl Controller for MockCameraController {
    type Config = MockCameraConfig;
    type Error = ControllerError;

    async fn init(config: Self::Config) -> Result<Self, Self::Error> {
        let intrinsics = CameraIntrinsics {
            width: config.width,
            height: config.height,
            fx: 500.0,
            fy: 500.0,
            cx: config.width as f64 / 2.0,
            cy: config.height as f64 / 2.0,
            distortion: vec![0.0; 5],
            distortion_model: "plumb_bob".to_string(),
        };

        Ok(Self {
            config,
            state: ControllerState::Initialized,
            frame_count: 0,
            intrinsics,
            parameters: CameraParameters::default(),
        })
    }

    async fn start(&mut self) -> Result<(), Self::Error> {
        self.state = ControllerState::Running;
        Ok(())
    }

    async fn stop(&mut self) -> Result<(), Self::Error> {
        self.state = ControllerState::Stopped;
        Ok(())
    }

    async fn health_check(&self) -> ControllerHealth {
        match self.state {
            ControllerState::Running => ControllerHealth::Healthy,
            ControllerState::Error => ControllerHealth::Unhealthy {
                reason: "Mock error".to_string(),
            },
            _ => ControllerHealth::Unknown,
        }
    }

    fn capabilities(&self) -> ControllerCapabilities {
        ControllerCapabilities::new("camera", "mock")
            .with_vendor("Mecha10")
            .with_model("Mock Camera")
            .with_feature("depth", self.config.has_depth)
            .with_feature("infrared", self.config.has_infrared)
            .with_feature("rgb", true)
    }
}

#[async_trait]
impl CameraController for MockCameraController {
    type Frame = CameraFrame;

    async fn capture_frame(&mut self) -> Result<Self::Frame, Self::Error> {
        if self.state != ControllerState::Running {
            return Err(ControllerError::InvalidState("Camera not running".to_string()));
        }

        self.frame_count += 1;

        let timestamp = SystemTime::now().duration_since(UNIX_EPOCH).unwrap().as_micros() as u64;

        // Generate synthetic RGB image (gradient pattern)
        let rgb = self.generate_rgb_image(timestamp);

        if self.config.has_depth {
            // Generate synthetic depth image
            let depth = self.generate_depth_image(timestamp);

            Ok(CameraFrame::Rgbd {
                color: rgb,
                depth,
                aligned: true,
            })
        } else {
            Ok(CameraFrame::Rgb(rgb))
        }
    }

    fn intrinsics(&self) -> Option<CameraIntrinsics> {
        Some(self.intrinsics.clone())
    }

    async fn set_parameters(&mut self, params: CameraParameters) -> Result<(), Self::Error> {
        self.parameters = params;
        Ok(())
    }

    async fn get_parameters(&self) -> Result<CameraParameters, Self::Error> {
        Ok(self.parameters.clone())
    }

    fn info(&self) -> CameraInfo {
        CameraInfo {
            timestamp: SystemTime::now().duration_since(UNIX_EPOCH).unwrap().as_micros() as u64,
            width: self.config.width,
            height: self.config.height,
            fps: self.config.fps as f32,
        }
    }
}

impl MockCameraController {
    /// Generate a synthetic RGB image with a gradient pattern
    fn generate_rgb_image(&self, timestamp: u64) -> RgbImage {
        let size = (self.config.width * self.config.height * 3) as usize;
        let mut data = vec![0u8; size];

        // Create a time-varying gradient pattern
        let phase = (timestamp / 1_000_000) % 256;

        for y in 0..self.config.height {
            for x in 0..self.config.width {
                let idx = ((y * self.config.width + x) * 3) as usize;
                data[idx] = ((x as u64 + phase) % 256) as u8; // R
                data[idx + 1] = ((y as u64 + phase) % 256) as u8; // G
                data[idx + 2] = ((x as u64 + y as u64 + phase) % 256) as u8; // B
            }
        }

        RgbImage {
            timestamp,
            frame_id: self.frame_count,
            width: self.config.width,
            height: self.config.height,
            encoding: "rgb8".to_string(),
            data,
        }
    }

    /// Generate a synthetic depth image with a radial pattern
    fn generate_depth_image(&self, timestamp: u64) -> DepthImage {
        let size = (self.config.width * self.config.height) as usize;
        let mut data = vec![0.0f32; size];

        let cx = self.config.width as f32 / 2.0;
        let cy = self.config.height as f32 / 2.0;

        for y in 0..self.config.height {
            for x in 0..self.config.width {
                let idx = (y * self.config.width + x) as usize;

                // Radial distance from center
                let dx = x as f32 - cx;
                let dy = y as f32 - cy;
                let dist = (dx * dx + dy * dy).sqrt();

                // Convert to depth (closer at center, farther at edges)
                let max_dist = (cx * cx + cy * cy).sqrt();
                data[idx] = 1.0 + (dist / max_dist) * 3.0; // 1-4 meters
            }
        }

        DepthImage {
            timestamp,
            frame_id: self.frame_count,
            width: self.config.width,
            height: self.config.height,
            data,
        }
    }

    /// Get the current frame count
    pub fn frame_count(&self) -> u64 {
        self.frame_count
    }

    /// Reset the frame count
    pub fn reset_frame_count(&mut self) {
        self.frame_count = 0;
    }
}