locus-core 0.4.0

#![allow(clippy::expect_used, clippy::unwrap_used)]
use rand_distr::{Distribution, Normal};

/// Generate a synthetic image containing a single AprilTag or ArUco tag.
///
/// This generates a tag with a white quiet zone, placed on a white background,
/// matching the setup used in Python benchmarks.
/// rotation (rad), translation (x,y), scaling (pixels).
#[must_use]
#[allow(
    clippy::cast_possible_truncation,
    clippy::cast_sign_loss,
    clippy::missing_panics_doc
)]
pub fn generate_synthetic_test_image(
    family: crate::config::TagFamily,
    id: u16,
    tag_size: usize,
    canvas_size: usize,
    noise_sigma: f32,
) -> (Vec<u8>, [[f64; 2]; 4]) {
    let mut data = vec![255u8; canvas_size * canvas_size];

    // Calculate tag position (centered)
    let margin = (canvas_size - tag_size) / 2;
    let quiet_zone = tag_size / 5;

    // Draw white quiet zone (optional since background is 255, but adds robustness)
    for y in margin.saturating_sub(quiet_zone)..(margin + tag_size + quiet_zone).min(canvas_size) {
        for x in
            margin.saturating_sub(quiet_zone)..(margin + tag_size + quiet_zone).min(canvas_size)
        {
            data[y * canvas_size + x] = 255;
        }
    }

    // Generate tag pattern (bits)
    let decoder = crate::decoder::family_to_decoder(family);
    let code = decoder.get_code(id).expect("Invalid tag ID for family");
    let dim = decoder.dimension();

    let cell_size = tag_size / (dim + 2); // dim + 2 for black border
    let actual_tag_size = cell_size * (dim + 2);
    let start_x = margin + (tag_size - actual_tag_size) / 2;
    let start_y = margin + (tag_size - actual_tag_size) / 2;

    // Draw black border
    for y in 0..(dim + 2) {
        for x in 0..(dim + 2) {
            if x == 0 || x == dim + 1 || y == 0 || y == dim + 1 {
                draw_cell(&mut data, canvas_size, start_x, start_y, x, y, cell_size, 0);
            }
        }
    }

    // Draw data bits using decoder's canonical sampling points
    let points = decoder.sample_points();
    let d_f = (dim + 2) as f64;
    for (i, p) in points.iter().enumerate() {
        // Map canonical [-1, 1] to grid index [0, D]
        let gx = ((p.0 + 1.0) * d_f / 2.0 - 0.5).round() as usize;
        let gy = ((p.1 + 1.0) * d_f / 2.0 - 0.5).round() as usize;

        let bit = (code >> i) & 1;
        let val = if bit != 0 { 255 } else { 0 };
        draw_cell(
            &mut data,
            canvas_size,
            start_x,
            start_y,
            gx,
            gy,
            cell_size,
            val,
        );
    }

    if noise_sigma > 0.0 {
        let mut rng = rand::rng();
        let normal = Normal::new(0.0, f64::from(noise_sigma)).expect("Invalid noise params");

        for pixel in &mut data {
            let noise = normal.sample(&mut rng) as i32;
            let val = (i32::from(*pixel) + noise).clamp(0, 255);
            *pixel = val as u8;
        }
    }

    let gt_corners = [
        [start_x as f64, start_y as f64],
        [(start_x + actual_tag_size) as f64, start_y as f64],
        [
            (start_x + actual_tag_size) as f64,
            (start_y + actual_tag_size) as f64,
        ],
        [start_x as f64, (start_y + actual_tag_size) as f64],
    ];

    (data, gt_corners)
}

#[allow(clippy::too_many_arguments)]
fn draw_cell(
    data: &mut [u8],
    stride: usize,
    start_x: usize,
    start_y: usize,
    cx: usize,
    cy: usize,
    size: usize,
    val: u8,
) {
    let px = start_x + cx * size;
    let py = start_y + cy * size;
    for y in py..(py + size) {
        for x in px..(px + size) {
            data[y * stride + x] = val;
        }
    }
}

/// Compute RMSE between detected and ground truth corners.
/// Use this when comparing against ground truth datasets with known corner ordering conventions.
#[must_use]
pub fn compute_corner_error(detected: &[[f64; 2]; 4], ground_truth: &[[f64; 2]; 4]) -> f64 {
    compute_rmse(detected, ground_truth)
}

/// Compute RMSE corner error between detected and ground truth corners.
///
/// Unlike `compute_corner_error`, this does NOT try rotations - it assumes corners
/// are already in the correct order. Use this when comparing against ground truth
/// datasets with known corner ordering conventions.
///
/// Formula: sqrt( (sum of squared distances for all 4 corners) / 4 )
#[must_use]
pub fn compute_rmse(detected: &[[f64; 2]; 4], ground_truth: &[[f64; 2]; 4]) -> f64 {
    let mut sum_sq = 0.0;
    for i in 0..4 {
        let dx = detected[i][0] - ground_truth[i][0];
        let dy = detected[i][1] - ground_truth[i][1];
        sum_sq += dx * dx + dy * dy;
    }
    (sum_sq / 4.0).sqrt()
}

// ============================================================================
// ROBUSTNESS TEST UTILITIES
// ============================================================================

/// Parameters for generating test images with photometric variations.
#[derive(Clone, Debug)]
#[allow(dead_code)]
pub(crate) struct TestImageParams {
    /// Tag family to generate.
    pub family: crate::config::TagFamily,
    /// Tag ID to generate.
    pub id: u16,
    /// Tag size in pixels.
    pub tag_size: usize,
    /// Canvas size in pixels.
    pub canvas_size: usize,
    /// Gaussian noise standard deviation (0.0 = no noise).
    pub noise_sigma: f32,
    /// Brightness offset (-255 to +255).
    pub brightness_offset: i16,
    /// Contrast scale (1.0 = no change, 0.5 = reduce, 1.5 = increase).
    pub contrast_scale: f32,
}

impl Default for TestImageParams {
    fn default() -> Self {
        Self {
            family: crate::config::TagFamily::AprilTag36h11,
            id: 0,
            tag_size: 100,
            canvas_size: 320,
            noise_sigma: 0.0,
            brightness_offset: 0,
            contrast_scale: 1.0,
        }
    }
}

/// Generate a test image based on the provided parameters.
/// Includes tag generation, placement, and photometric adjustments.
#[must_use]
#[allow(clippy::cast_possible_truncation, clippy::cast_sign_loss)]
#[allow(dead_code)]
pub(crate) fn generate_test_image_with_params(
    params: &TestImageParams,
) -> (Vec<u8>, [[f64; 2]; 4]) {
    // First generate base image
    let (mut data, corners) = generate_synthetic_test_image(
        params.family,
        params.id,
        params.tag_size,
        params.canvas_size,
        params.noise_sigma,
    );

    // Apply brightness and contrast adjustments
    if params.brightness_offset != 0 || (params.contrast_scale - 1.0).abs() > 0.001 {
        apply_brightness_contrast(
            &mut data,
            i32::from(params.brightness_offset),
            params.contrast_scale,
        );
    }

    (data, corners)
}

/// Apply brightness and contrast to an image.
/// `brightness`: -255 to +255
/// `contrast`: 0.0 to 127.0 (1.0 = no change)
#[allow(clippy::cast_possible_wrap, clippy::cast_sign_loss)]
#[allow(dead_code)]
pub(crate) fn apply_brightness_contrast(image: &mut [u8], brightness: i32, contrast: f32) {
    for pixel in image.iter_mut() {
        let b = f32::from(*pixel);
        let with_contrast = (b - 128.0) * contrast + 128.0;
        let with_brightness = with_contrast as i32 + brightness;
        *pixel = with_brightness.clamp(0, 255) as u8;
    }
}

/// Count black pixels in binary data.
#[must_use]
#[allow(clippy::naive_bytecount)]
#[allow(dead_code)]
pub(crate) fn count_black_pixels(data: &[u8]) -> usize {
    data.iter().filter(|&&p| p == 0).count()
}

/// Check if the tag's outer black border is correctly binarized.
/// Returns the ratio of correctly black pixels in the 1-cell-wide border (0.0 to 1.0).
#[must_use]
#[allow(clippy::cast_sign_loss)]
#[allow(dead_code)]
pub(crate) fn measure_border_integrity(
    binary: &[u8],
    width: usize,
    corners: &[[f64; 2]; 4],
) -> f64 {
    let min_x = corners
        .iter()
        .map(|c| c[0])
        .fold(f64::MAX, f64::min)
        .max(0.0) as usize;
    let max_x = corners
        .iter()
        .map(|c| c[0])
        .fold(f64::MIN, f64::max)
        .max(0.0) as usize;
    let min_y = corners
        .iter()
        .map(|c| c[1])
        .fold(f64::MAX, f64::min)
        .max(0.0) as usize;
    let max_y = corners
        .iter()
        .map(|c| c[1])
        .fold(f64::MIN, f64::max)
        .max(0.0) as usize;

    let height = binary.len() / width;
    let min_x = min_x.min(width.saturating_sub(1));
    let max_x = max_x.min(width.saturating_sub(1));
    let min_y = min_y.min(height.saturating_sub(1));
    let max_y = max_y.min(height.saturating_sub(1));

    if max_x <= min_x || max_y <= min_y {
        return 0.0;
    }

    let tag_width = max_x - min_x;
    let tag_height = max_y - min_y;

    // For AprilTag 36h11: 8 cells total, border is 1 cell = 1/8 of tag
    let cell_size_x = tag_width / 8;
    let cell_size_y = tag_height / 8;

    if cell_size_x == 0 || cell_size_y == 0 {
        return 0.0;
    }

    let mut black_count = 0usize;
    let mut total_count = 0usize;

    // Top border row
    for y in min_y..(min_y + cell_size_y).min(max_y) {
        for x in min_x..=max_x {
            if y < height && x < width {
                total_count += 1;
                if binary[y * width + x] == 0 {
                    black_count += 1;
                }
            }
        }
    }

    // Bottom border row
    let bottom_start = max_y.saturating_sub(cell_size_y);
    for y in bottom_start..=max_y {
        for x in min_x..=max_x {
            if y < height && x < width {
                total_count += 1;
                if binary[y * width + x] == 0 {
                    black_count += 1;
                }
            }
        }
    }

    // Left border column (excluding corners)
    for y in (min_y + cell_size_y)..(max_y.saturating_sub(cell_size_y)) {
        for x in min_x..(min_x + cell_size_x).min(max_x) {
            if y < height && x < width {
                total_count += 1;
                if binary[y * width + x] == 0 {
                    black_count += 1;
                }
            }
        }
    }

    // Right border column (excluding corners)
    let right_start = max_x.saturating_sub(cell_size_x);
    for y in (min_y + cell_size_y)..(max_y.saturating_sub(cell_size_y)) {
        for x in right_start..=max_x {
            if y < height && x < width {
                total_count += 1;
                if binary[y * width + x] == 0 {
                    black_count += 1;
                }
            }
        }
    }

    if total_count == 0 {
        0.0
    } else {
        black_count as f64 / total_count as f64
    }
}

/// Generates a checkered pattern image for benchmarking.
#[must_use]
#[cfg(feature = "bench-internals")]
pub fn generate_checkered(width: usize, height: usize) -> Vec<u8> {
    let mut data = vec![200u8; width * height];
    for y in (0..height).step_by(16) {
        for x in (0..width).step_by(16) {
            if ((x / 16) + (y / 16)) % 2 == 0 {
                for dy in 0..16 {
                    if y + dy < height {
                        let row_off = (y + dy) * width;
                        for dx in 0..16 {
                            if x + dx < width {
                                data[row_off + x + dx] = 50;
                            }
                        }
                    }
                }
            }
        }
    }
    data
}

/// Sub-pixel edge rendering utilities for accuracy testing.
pub mod subpixel;

/// Complex multi-tag scene generation for integration testing.
#[cfg(any(feature = "extended-tests", feature = "extended-bench"))]
pub mod scene;
#[cfg(any(feature = "extended-tests", feature = "extended-bench"))]
pub use scene::{SceneBuilder, TagPlacement};