corrmatch 0.1.0

//! Refinement search around coarse candidates.
//!
//! Refinement upsamples coarse candidates to finer levels and searches a local
//! ROI and angle neighborhood to improve position and angle estimates.

use crate::bank::CompiledTemplate;
use crate::candidate::nms::nms_2d;
use crate::candidate::topk::{Peak, TopK};
use crate::image::integral::IntegralImages;
use crate::kernel::scalar::{SsdMaskedScalar, ZnccMaskedScalar};
use crate::kernel::{Kernel, ScanParams, ScanRoi};
use crate::refine::quad1d::quad_peak_offset_1d;
use crate::refine::quad2d::refine_subpixel_2d;
use crate::search::{Match, MatchConfig, Metric};
use crate::util::math::wrap_deg;
use crate::util::{CorrMatchError, CorrMatchResult};
use crate::ImageView;

#[cfg(feature = "rayon")]
use rayon::prelude::*;

// Kernel type aliases for unmasked kernels - use SIMD when available
#[cfg(not(feature = "simd"))]
use crate::kernel::scalar::{SsdUnmaskedScalar as SsdUnmasked, ZnccUnmaskedScalar as ZnccUnmasked};
#[cfg(feature = "simd")]
use crate::kernel::simd::{SsdUnmaskedSimd as SsdUnmasked, ZnccUnmaskedSimd as ZnccUnmasked};
#[derive(Clone, Copy, Debug)]
pub(crate) struct Candidate {
    pub(crate) level: usize,
    pub(crate) x: usize,
    pub(crate) y: usize,
    pub(crate) angle_idx: usize,
    pub(crate) angle_deg: f32,
    pub(crate) score: f32,
}

impl Candidate {
    pub(crate) fn to_peak(self) -> Peak {
        Peak {
            x: self.x,
            y: self.y,
            score: self.score,
            angle_idx: self.angle_idx,
        }
    }

    pub(crate) fn from_peak(level: usize, angle_deg: f32, peak: Peak) -> Self {
        Self {
            level,
            x: peak.x,
            y: peak.y,
            angle_idx: peak.angle_idx,
            angle_deg,
            score: peak.score,
        }
    }
}

fn upscale_pos(x: usize, y: usize) -> (usize, usize) {
    (x.saturating_mul(2), y.saturating_mul(2))
}

fn roi_bounds(
    x: usize,
    y: usize,
    radius: usize,
    max_x: usize,
    max_y: usize,
) -> Option<(usize, usize, usize, usize)> {
    let mut x0 = x.saturating_sub(radius);
    let mut y0 = y.saturating_sub(radius);
    let mut x1 = x.saturating_add(radius);
    let mut y1 = y.saturating_add(radius);

    if x0 > max_x || y0 > max_y {
        return None;
    }

    x0 = x0.min(max_x);
    y0 = y0.min(max_y);
    x1 = x1.min(max_x);
    y1 = y1.min(max_y);

    if x0 > x1 || y0 > y1 {
        return None;
    }

    Some((x0, y0, x1, y1))
}

/// Original per-angle refinement (kept for reference and potential fallback).
#[allow(dead_code)]
pub(crate) fn refine_to_finer_level(
    image: ImageView<'_, u8>,
    compiled: &CompiledTemplate,
    finer_level: usize,
    prev: &[Candidate],
    cfg: &MatchConfig,
) -> CorrMatchResult<Vec<Candidate>> {
    let _span = trace_span!("refine_level", level = finer_level, candidates = prev.len()).entered();

    if prev.is_empty() {
        return Ok(Vec::new());
    }

    let grid = compiled
        .angle_grid(finer_level)
        .ok_or(CorrMatchError::IndexOutOfBounds {
            index: finer_level,
            len: compiled.num_levels(),
            context: "level",
        })?;
    let (tpl_width, tpl_height) =
        compiled
            .level_size(finer_level)
            .ok_or(CorrMatchError::IndexOutOfBounds {
                index: finer_level,
                len: compiled.num_levels(),
                context: "level",
            })?;

    let img_width = image.width();
    let img_height = image.height();
    if img_width < tpl_width || img_height < tpl_height {
        return Err(CorrMatchError::RoiOutOfBounds {
            x: 0,
            y: 0,
            width: tpl_width,
            height: tpl_height,
            img_width,
            img_height,
        });
    }

    let max_x = img_width - tpl_width;
    let max_y = img_height - tpl_height;
    let params = ScanParams {
        topk: cfg.per_angle_topk,
        min_var_i: cfg.min_var_i,
        min_score: cfg.min_score,
    };
    let mut all_peaks = Vec::new();

    for cand in prev.iter().copied() {
        debug_assert!(cand.level > finer_level);
        let (x_up, y_up) = upscale_pos(cand.x, cand.y);
        let roi = match roi_bounds(x_up, y_up, cfg.roi_radius, max_x, max_y) {
            Some(bounds) => bounds,
            None => continue,
        };

        let half_range = cfg.angle_half_range_steps as f32 * grid.step_deg();
        let angle_indices = grid.indices_within(cand.angle_deg, half_range);
        for angle_idx in angle_indices {
            let peaks = match cfg.metric {
                Metric::Zncc => {
                    let plan = compiled.rotated_zncc_plan(finer_level, angle_idx)?;
                    <ZnccMaskedScalar as Kernel>::scan_roi(
                        image, plan, angle_idx, roi.0, roi.1, roi.2, roi.3, params,
                    )?
                }
                Metric::Ssd => {
                    let plan = compiled.rotated_ssd_plan(finer_level, angle_idx)?;
                    <SsdMaskedScalar as Kernel>::scan_roi(
                        image, plan, angle_idx, roi.0, roi.1, roi.2, roi.3, params,
                    )?
                }
            };
            all_peaks.extend(peaks);
        }
    }

    if all_peaks.is_empty() {
        return Ok(Vec::new());
    }

    let mut kept = nms_2d(&mut all_peaks, cfg.nms_radius);
    if kept.len() > cfg.beam_width {
        kept.truncate(cfg.beam_width);
    }

    let mut out = Vec::with_capacity(kept.len());
    for peak in kept.drain(..) {
        let angle_deg = grid.angle_at(peak.angle_idx);
        out.push(Candidate::from_peak(finer_level, angle_deg, peak));
    }

    trace_event!("refined_candidates", count = out.len());
    Ok(out)
}

/// Batch refinement with multi-angle processing for cache locality.
///
/// This function processes all angles at each (x, y) position before moving to the next,
/// which avoids redundant image row fetches. Each image row is loaded once and reused
/// across all angle evaluations at that position.
///
/// Expected speedup: 30-50% for rotation-enabled matching compared to per-angle scanning.
pub(crate) fn refine_to_finer_level_batch(
    image: ImageView<'_, u8>,
    compiled: &CompiledTemplate,
    finer_level: usize,
    prev: &[Candidate],
    cfg: &MatchConfig,
) -> CorrMatchResult<Vec<Candidate>> {
    let _span = trace_span!(
        "refine_level_batch",
        level = finer_level,
        candidates = prev.len()
    )
    .entered();

    if prev.is_empty() {
        return Ok(Vec::new());
    }

    let grid = compiled
        .angle_grid(finer_level)
        .ok_or(CorrMatchError::IndexOutOfBounds {
            index: finer_level,
            len: compiled.num_levels(),
            context: "level",
        })?;
    let (tpl_width, tpl_height) =
        compiled
            .level_size(finer_level)
            .ok_or(CorrMatchError::IndexOutOfBounds {
                index: finer_level,
                len: compiled.num_levels(),
                context: "level",
            })?;

    let img_width = image.width();
    let img_height = image.height();
    if img_width < tpl_width || img_height < tpl_height {
        return Err(CorrMatchError::RoiOutOfBounds {
            x: 0,
            y: 0,
            width: tpl_width,
            height: tpl_height,
            img_width,
            img_height,
        });
    }

    let max_x = img_width - tpl_width;
    let max_y = img_height - tpl_height;
    let min_var_i = cfg.min_var_i;
    let min_score = cfg.min_score;
    let mut all_peaks = Vec::new();
    let mut cached_rows: Vec<&[u8]> = Vec::with_capacity(tpl_height);

    // Process each candidate's ROI with batch angle evaluation
    for cand in prev.iter().copied() {
        debug_assert!(cand.level > finer_level);
        let (x_up, y_up) = upscale_pos(cand.x, cand.y);
        let roi = match roi_bounds(x_up, y_up, cfg.roi_radius, max_x, max_y) {
            Some(bounds) => bounds,
            None => continue,
        };

        // Collect angle indices for this candidate
        let half_range = cfg.angle_half_range_steps as f32 * grid.step_deg();
        let angle_indices = grid.indices_within(cand.angle_deg, half_range);
        if angle_indices.is_empty() {
            continue;
        }

        // Iterate over positions: for each (x, y), evaluate ALL angles with cached rows
        let (x0, y0, x1, y1) = roi;
        match cfg.metric {
            Metric::Zncc => {
                let mut angle_plans = Vec::with_capacity(angle_indices.len());
                for &angle_idx in &angle_indices {
                    let plan = compiled.rotated_zncc_plan(finer_level, angle_idx)?;
                    angle_plans.push((angle_idx, plan, TopK::new(cfg.per_angle_topk)));
                }

                for y in y0..=y1 {
                    cached_rows.clear();
                    for ty in 0..tpl_height {
                        cached_rows.push(image.row(y + ty).expect("row within bounds"));
                    }

                    for x in x0..=x1 {
                        for (angle_idx, plan, topk) in angle_plans.iter_mut() {
                            let score =
                                ZnccMaskedScalar::score_at_cached(&cached_rows, plan, x, min_var_i);
                            if score.is_finite() && score >= min_score {
                                topk.push(Peak {
                                    x,
                                    y,
                                    score,
                                    angle_idx: *angle_idx,
                                });
                            }
                        }
                    }
                }

                for (_angle_idx, _plan, topk) in angle_plans {
                    all_peaks.extend(topk.into_sorted_desc());
                }
            }
            Metric::Ssd => {
                let mut angle_plans = Vec::with_capacity(angle_indices.len());
                for &angle_idx in &angle_indices {
                    let plan = compiled.rotated_ssd_plan(finer_level, angle_idx)?;
                    angle_plans.push((angle_idx, plan, TopK::new(cfg.per_angle_topk)));
                }

                for y in y0..=y1 {
                    cached_rows.clear();
                    for ty in 0..tpl_height {
                        cached_rows.push(image.row(y + ty).expect("row within bounds"));
                    }

                    for x in x0..=x1 {
                        for (angle_idx, plan, topk) in angle_plans.iter_mut() {
                            let score = SsdMaskedScalar::score_at_cached(&cached_rows, plan, x);
                            if score.is_finite() && score >= min_score {
                                topk.push(Peak {
                                    x,
                                    y,
                                    score,
                                    angle_idx: *angle_idx,
                                });
                            }
                        }
                    }
                }

                for (_angle_idx, _plan, topk) in angle_plans {
                    all_peaks.extend(topk.into_sorted_desc());
                }
            }
        }
    }

    if all_peaks.is_empty() {
        return Ok(Vec::new());
    }

    let mut kept = nms_2d(&mut all_peaks, cfg.nms_radius);
    if kept.len() > cfg.beam_width {
        kept.truncate(cfg.beam_width);
    }

    let mut out = Vec::with_capacity(kept.len());
    for peak in kept.drain(..) {
        let angle_deg = grid.angle_at(peak.angle_idx);
        out.push(Candidate::from_peak(finer_level, angle_deg, peak));
    }

    trace_event!("refined_candidates", count = out.len());
    Ok(out)
}

/// Refines candidates without rotation by scanning only translation ROIs.
pub(crate) fn refine_to_finer_level_unmasked(
    image: ImageView<'_, u8>,
    compiled: &CompiledTemplate,
    finer_level: usize,
    prev: &[Candidate],
    cfg: &MatchConfig,
) -> CorrMatchResult<Vec<Candidate>> {
    let _span = trace_span!("refine_level", level = finer_level, candidates = prev.len()).entered();

    if prev.is_empty() {
        return Ok(Vec::new());
    }

    let (tpl_width, tpl_height) =
        compiled
            .level_size(finer_level)
            .ok_or(CorrMatchError::IndexOutOfBounds {
                index: finer_level,
                len: compiled.num_levels(),
                context: "level",
            })?;

    let img_width = image.width();
    let img_height = image.height();
    if img_width < tpl_width || img_height < tpl_height {
        return Err(CorrMatchError::RoiOutOfBounds {
            x: 0,
            y: 0,
            width: tpl_width,
            height: tpl_height,
            img_width,
            img_height,
        });
    }

    let max_x = img_width - tpl_width;
    let max_y = img_height - tpl_height;
    let params = ScanParams {
        topk: cfg.per_angle_topk,
        min_var_i: cfg.min_var_i,
        min_score: cfg.min_score,
    };
    let mut all_peaks = Vec::new();

    match cfg.metric {
        Metric::Zncc => {
            let plan = compiled.unmasked_zncc_plan(finer_level)?;
            for cand in prev.iter().copied() {
                debug_assert!(cand.level > finer_level);
                let (x_up, y_up) = upscale_pos(cand.x, cand.y);
                let roi = match roi_bounds(x_up, y_up, cfg.roi_radius, max_x, max_y) {
                    Some(bounds) => bounds,
                    None => continue,
                };
                let peaks = <ZnccUnmasked as Kernel>::scan_roi(
                    image, plan, 0, roi.0, roi.1, roi.2, roi.3, params,
                )?;
                all_peaks.extend(peaks);
            }
        }
        Metric::Ssd => {
            let plan = compiled.unmasked_ssd_plan(finer_level)?;
            for cand in prev.iter().copied() {
                debug_assert!(cand.level > finer_level);
                let (x_up, y_up) = upscale_pos(cand.x, cand.y);
                let roi = match roi_bounds(x_up, y_up, cfg.roi_radius, max_x, max_y) {
                    Some(bounds) => bounds,
                    None => continue,
                };
                let peaks = <SsdUnmasked as Kernel>::scan_roi(
                    image, plan, 0, roi.0, roi.1, roi.2, roi.3, params,
                )?;
                all_peaks.extend(peaks);
            }
        }
    }

    if all_peaks.is_empty() {
        return Ok(Vec::new());
    }

    let mut kept = nms_2d(&mut all_peaks, cfg.nms_radius);
    if kept.len() > cfg.beam_width {
        kept.truncate(cfg.beam_width);
    }

    let mut out = Vec::with_capacity(kept.len());
    for peak in kept.drain(..) {
        out.push(Candidate::from_peak(finer_level, 0.0, peak));
    }

    trace_event!("refined_candidates", count = out.len());
    Ok(out)
}

/// Refines candidates without rotation using integral-image variance pruning.
pub(crate) fn refine_to_finer_level_unmasked_zncc_integral(
    image: ImageView<'_, u8>,
    compiled: &CompiledTemplate,
    finer_level: usize,
    prev: &[Candidate],
    cfg: &MatchConfig,
    integrals: &IntegralImages,
) -> CorrMatchResult<Vec<Candidate>> {
    let _span = trace_span!("refine_level", level = finer_level, candidates = prev.len()).entered();

    if prev.is_empty() {
        return Ok(Vec::new());
    }

    debug_assert!(matches!(cfg.metric, Metric::Zncc));
    let (tpl_width, tpl_height) =
        compiled
            .level_size(finer_level)
            .ok_or(CorrMatchError::IndexOutOfBounds {
                index: finer_level,
                len: compiled.num_levels(),
                context: "level",
            })?;

    let img_width = image.width();
    let img_height = image.height();
    if img_width < tpl_width || img_height < tpl_height {
        return Err(CorrMatchError::RoiOutOfBounds {
            x: 0,
            y: 0,
            width: tpl_width,
            height: tpl_height,
            img_width,
            img_height,
        });
    }

    let max_x = img_width - tpl_width;
    let max_y = img_height - tpl_height;
    let params = ScanParams {
        topk: cfg.per_angle_topk,
        min_var_i: cfg.min_var_i,
        min_score: cfg.min_score,
    };
    let mut all_peaks = Vec::new();

    let plan = compiled.unmasked_zncc_plan(finer_level)?;
    for cand in prev.iter().copied() {
        debug_assert!(cand.level > finer_level);
        let (x_up, y_up) = upscale_pos(cand.x, cand.y);
        let roi = match roi_bounds(x_up, y_up, cfg.roi_radius, max_x, max_y) {
            Some(bounds) => bounds,
            None => continue,
        };
        let peaks = ZnccUnmasked::scan_roi_integral(
            image,
            plan,
            0,
            ScanRoi::new(roi.0, roi.1, roi.2, roi.3),
            params,
            integrals,
        )?;
        all_peaks.extend(peaks);
    }

    if all_peaks.is_empty() {
        return Ok(Vec::new());
    }

    let mut kept = nms_2d(&mut all_peaks, cfg.nms_radius);
    if kept.len() > cfg.beam_width {
        kept.truncate(cfg.beam_width);
    }

    let mut out = Vec::with_capacity(kept.len());
    for peak in kept.drain(..) {
        out.push(Candidate::from_peak(finer_level, 0.0, peak));
    }

    trace_event!("refined_candidates", count = out.len());
    Ok(out)
}

/// Refinement search around coarse candidates (parallel).
#[cfg(feature = "rayon")]
pub(crate) fn refine_to_finer_level_par(
    image: ImageView<'_, u8>,
    compiled: &CompiledTemplate,
    finer_level: usize,
    prev: &[Candidate],
    cfg: &MatchConfig,
) -> CorrMatchResult<Vec<Candidate>> {
    let _span = trace_span!(
        "refine_level",
        level = finer_level,
        candidates = prev.len(),
        parallel = true
    )
    .entered();

    if prev.is_empty() {
        return Ok(Vec::new());
    }

    let grid = compiled
        .angle_grid(finer_level)
        .ok_or(CorrMatchError::IndexOutOfBounds {
            index: finer_level,
            len: compiled.num_levels(),
            context: "level",
        })?;
    let (tpl_width, tpl_height) =
        compiled
            .level_size(finer_level)
            .ok_or(CorrMatchError::IndexOutOfBounds {
                index: finer_level,
                len: compiled.num_levels(),
                context: "level",
            })?;

    let img_width = image.width();
    let img_height = image.height();
    if img_width < tpl_width || img_height < tpl_height {
        return Err(CorrMatchError::RoiOutOfBounds {
            x: 0,
            y: 0,
            width: tpl_width,
            height: tpl_height,
            img_width,
            img_height,
        });
    }

    let max_x = img_width - tpl_width;
    let max_y = img_height - tpl_height;

    let results: Vec<_> = prev
        .par_iter()
        .copied()
        .map(|cand| {
            debug_assert!(cand.level > finer_level);
            let (x_up, y_up) = upscale_pos(cand.x, cand.y);
            let roi = match roi_bounds(x_up, y_up, cfg.roi_radius, max_x, max_y) {
                Some(bounds) => bounds,
                None => return Ok(Vec::new()),
            };

            let half_range = cfg.angle_half_range_steps as f32 * grid.step_deg();
            let angle_indices = grid.indices_within(cand.angle_deg, half_range);
            let mut local_peaks = Vec::new();
            let params = ScanParams {
                topk: cfg.per_angle_topk,
                min_var_i: cfg.min_var_i,
                min_score: cfg.min_score,
            };
            for angle_idx in angle_indices {
                let peaks = match cfg.metric {
                    Metric::Zncc => {
                        let plan = compiled.rotated_zncc_plan(finer_level, angle_idx)?;
                        <ZnccMaskedScalar as Kernel>::scan_roi(
                            image, plan, angle_idx, roi.0, roi.1, roi.2, roi.3, params,
                        )?
                    }
                    Metric::Ssd => {
                        let plan = compiled.rotated_ssd_plan(finer_level, angle_idx)?;
                        <SsdMaskedScalar as Kernel>::scan_roi(
                            image, plan, angle_idx, roi.0, roi.1, roi.2, roi.3, params,
                        )?
                    }
                };
                local_peaks.extend(peaks);
            }

            Ok(local_peaks)
        })
        .collect();

    let mut all_peaks = Vec::new();
    for result in results {
        all_peaks.extend(result?);
    }
    if all_peaks.is_empty() {
        return Ok(Vec::new());
    }

    let mut kept = nms_2d(&mut all_peaks, cfg.nms_radius);
    if kept.len() > cfg.beam_width {
        kept.truncate(cfg.beam_width);
    }

    let mut out = Vec::with_capacity(kept.len());
    for peak in kept.drain(..) {
        let angle_deg = grid.angle_at(peak.angle_idx);
        out.push(Candidate::from_peak(finer_level, angle_deg, peak));
    }

    trace_event!("refined_candidates", count = out.len());
    Ok(out)
}

/// Refines candidates without rotation by scanning only translation ROIs (parallel).
#[cfg(feature = "rayon")]
pub(crate) fn refine_to_finer_level_unmasked_par(
    image: ImageView<'_, u8>,
    compiled: &CompiledTemplate,
    finer_level: usize,
    prev: &[Candidate],
    cfg: &MatchConfig,
) -> CorrMatchResult<Vec<Candidate>> {
    let _span = trace_span!(
        "refine_level",
        level = finer_level,
        candidates = prev.len(),
        parallel = true
    )
    .entered();

    if prev.is_empty() {
        return Ok(Vec::new());
    }

    let (tpl_width, tpl_height) =
        compiled
            .level_size(finer_level)
            .ok_or(CorrMatchError::IndexOutOfBounds {
                index: finer_level,
                len: compiled.num_levels(),
                context: "level",
            })?;

    let img_width = image.width();
    let img_height = image.height();
    if img_width < tpl_width || img_height < tpl_height {
        return Err(CorrMatchError::RoiOutOfBounds {
            x: 0,
            y: 0,
            width: tpl_width,
            height: tpl_height,
            img_width,
            img_height,
        });
    }

    let max_x = img_width - tpl_width;
    let max_y = img_height - tpl_height;
    let params = ScanParams {
        topk: cfg.per_angle_topk,
        min_var_i: cfg.min_var_i,
        min_score: cfg.min_score,
    };

    let results: Vec<_> = match cfg.metric {
        Metric::Zncc => {
            let plan = compiled.unmasked_zncc_plan(finer_level)?;
            prev.par_iter()
                .copied()
                .map(|cand| {
                    debug_assert!(cand.level > finer_level);
                    let (x_up, y_up) = upscale_pos(cand.x, cand.y);
                    let roi = match roi_bounds(x_up, y_up, cfg.roi_radius, max_x, max_y) {
                        Some(bounds) => bounds,
                        None => return Ok(Vec::new()),
                    };
                    <ZnccUnmasked as Kernel>::scan_roi(
                        image, plan, 0, roi.0, roi.1, roi.2, roi.3, params,
                    )
                })
                .collect()
        }
        Metric::Ssd => {
            let plan = compiled.unmasked_ssd_plan(finer_level)?;
            prev.par_iter()
                .copied()
                .map(|cand| {
                    debug_assert!(cand.level > finer_level);
                    let (x_up, y_up) = upscale_pos(cand.x, cand.y);
                    let roi = match roi_bounds(x_up, y_up, cfg.roi_radius, max_x, max_y) {
                        Some(bounds) => bounds,
                        None => return Ok(Vec::new()),
                    };
                    <SsdUnmasked as Kernel>::scan_roi(
                        image, plan, 0, roi.0, roi.1, roi.2, roi.3, params,
                    )
                })
                .collect()
        }
    };

    let mut all_peaks = Vec::new();
    for result in results {
        all_peaks.extend(result?);
    }
    if all_peaks.is_empty() {
        return Ok(Vec::new());
    }

    let mut kept = nms_2d(&mut all_peaks, cfg.nms_radius);
    if kept.len() > cfg.beam_width {
        kept.truncate(cfg.beam_width);
    }

    let mut out = Vec::with_capacity(kept.len());
    for peak in kept.drain(..) {
        out.push(Candidate::from_peak(finer_level, 0.0, peak));
    }

    trace_event!("refined_candidates", count = out.len());
    Ok(out)
}

/// Refines the best candidate at the finest level with subpixel and subangle fits.
pub(crate) fn refine_final_match(
    image: ImageView<'_, u8>,
    compiled: &CompiledTemplate,
    level: usize,
    best: Candidate,
    cfg: &MatchConfig,
) -> CorrMatchResult<Match> {
    let _span = trace_span!("final_refinement").entered();

    let grid = compiled
        .angle_grid(level)
        .ok_or(CorrMatchError::IndexOutOfBounds {
            index: level,
            len: compiled.num_levels(),
            context: "level",
        })?;
    let (tpl_width, tpl_height) =
        compiled
            .level_size(level)
            .ok_or(CorrMatchError::IndexOutOfBounds {
                index: level,
                len: compiled.num_levels(),
                context: "level",
            })?;

    let img_width = image.width();
    let img_height = image.height();
    if img_width < tpl_width || img_height < tpl_height {
        return Err(CorrMatchError::RoiOutOfBounds {
            x: best.x,
            y: best.y,
            width: tpl_width,
            height: tpl_height,
            img_width,
            img_height,
        });
    }
    let max_x = img_width - tpl_width;
    let max_y = img_height - tpl_height;
    if best.x > max_x || best.y > max_y {
        return Err(CorrMatchError::RoiOutOfBounds {
            x: best.x,
            y: best.y,
            width: tpl_width,
            height: tpl_height,
            img_width,
            img_height,
        });
    }

    let mut s = [[f32::NEG_INFINITY; 3]; 3];
    let offsets = [-1isize, 0, 1];
    let (center_score, sm, sp) = match cfg.metric {
        Metric::Zncc => {
            let plan = compiled.rotated_zncc_plan(level, best.angle_idx)?;
            for (iy, &dy) in offsets.iter().enumerate() {
                let y = best.y as isize + dy;
                if y < 0 || y > max_y as isize {
                    continue;
                }
                for (ix, &dx) in offsets.iter().enumerate() {
                    let x = best.x as isize + dx;
                    if x < 0 || x > max_x as isize {
                        continue;
                    }
                    s[iy][ix] = <ZnccMaskedScalar as Kernel>::score_at(
                        image,
                        plan,
                        x as usize,
                        y as usize,
                        cfg.min_var_i,
                    );
                }
            }

            let len = grid.len();
            let im = (best.angle_idx + len - 1) % len;
            let ip = (best.angle_idx + 1) % len;
            let sm = <ZnccMaskedScalar as Kernel>::score_at(
                image,
                compiled.rotated_zncc_plan(level, im)?,
                best.x,
                best.y,
                cfg.min_var_i,
            );
            let sp = <ZnccMaskedScalar as Kernel>::score_at(
                image,
                compiled.rotated_zncc_plan(level, ip)?,
                best.x,
                best.y,
                cfg.min_var_i,
            );
            (s[1][1], sm, sp)
        }
        Metric::Ssd => {
            let plan = compiled.rotated_ssd_plan(level, best.angle_idx)?;
            for (iy, &dy) in offsets.iter().enumerate() {
                let y = best.y as isize + dy;
                if y < 0 || y > max_y as isize {
                    continue;
                }
                for (ix, &dx) in offsets.iter().enumerate() {
                    let x = best.x as isize + dx;
                    if x < 0 || x > max_x as isize {
                        continue;
                    }
                    s[iy][ix] = <SsdMaskedScalar as Kernel>::score_at(
                        image,
                        plan,
                        x as usize,
                        y as usize,
                        cfg.min_var_i,
                    );
                }
            }

            let len = grid.len();
            let im = (best.angle_idx + len - 1) % len;
            let ip = (best.angle_idx + 1) % len;
            let sm = <SsdMaskedScalar as Kernel>::score_at(
                image,
                compiled.rotated_ssd_plan(level, im)?,
                best.x,
                best.y,
                cfg.min_var_i,
            );
            let sp = <SsdMaskedScalar as Kernel>::score_at(
                image,
                compiled.rotated_ssd_plan(level, ip)?,
                best.x,
                best.y,
                cfg.min_var_i,
            );
            (s[1][1], sm, sp)
        }
    };

    let center_score = if center_score.is_finite() {
        center_score
    } else {
        best.score
    };
    let (x_ref, y_ref) = refine_subpixel_2d(best.x, best.y, s);

    let len = grid.len();
    debug_assert!(len > 0);
    let center_angle = grid.angle_at(best.angle_idx);
    let step = grid.step_deg();
    let angle_offset = quad_peak_offset_1d(sm, center_score, sp).unwrap_or(0.0);
    let angle_deg = wrap_deg(center_angle + angle_offset * step);

    Ok(Match {
        x: x_ref,
        y: y_ref,
        angle_deg,
        score: center_score,
    })
}

/// Refines the best candidate at the finest level without rotation.
pub(crate) fn refine_final_match_unmasked(
    image: ImageView<'_, u8>,
    compiled: &CompiledTemplate,
    level: usize,
    best: Candidate,
    cfg: &MatchConfig,
) -> CorrMatchResult<Match> {
    let _span = trace_span!("final_refinement").entered();

    let (tpl_width, tpl_height) =
        compiled
            .level_size(level)
            .ok_or(CorrMatchError::IndexOutOfBounds {
                index: level,
                len: compiled.num_levels(),
                context: "level",
            })?;

    let img_width = image.width();
    let img_height = image.height();
    if img_width < tpl_width || img_height < tpl_height {
        return Err(CorrMatchError::RoiOutOfBounds {
            x: best.x,
            y: best.y,
            width: tpl_width,
            height: tpl_height,
            img_width,
            img_height,
        });
    }
    let max_x = img_width - tpl_width;
    let max_y = img_height - tpl_height;
    if best.x > max_x || best.y > max_y {
        return Err(CorrMatchError::RoiOutOfBounds {
            x: best.x,
            y: best.y,
            width: tpl_width,
            height: tpl_height,
            img_width,
            img_height,
        });
    }

    let mut s = [[f32::NEG_INFINITY; 3]; 3];
    let offsets = [-1isize, 0, 1];
    match cfg.metric {
        Metric::Zncc => {
            let plan = compiled.unmasked_zncc_plan(level)?;
            for (iy, &dy) in offsets.iter().enumerate() {
                let y = best.y as isize + dy;
                if y < 0 || y > max_y as isize {
                    continue;
                }
                for (ix, &dx) in offsets.iter().enumerate() {
                    let x = best.x as isize + dx;
                    if x < 0 || x > max_x as isize {
                        continue;
                    }
                    s[iy][ix] = <ZnccUnmasked as Kernel>::score_at(
                        image,
                        plan,
                        x as usize,
                        y as usize,
                        cfg.min_var_i,
                    );
                }
            }
        }
        Metric::Ssd => {
            let plan = compiled.unmasked_ssd_plan(level)?;
            for (iy, &dy) in offsets.iter().enumerate() {
                let y = best.y as isize + dy;
                if y < 0 || y > max_y as isize {
                    continue;
                }
                for (ix, &dx) in offsets.iter().enumerate() {
                    let x = best.x as isize + dx;
                    if x < 0 || x > max_x as isize {
                        continue;
                    }
                    s[iy][ix] = <SsdUnmasked as Kernel>::score_at(
                        image,
                        plan,
                        x as usize,
                        y as usize,
                        cfg.min_var_i,
                    );
                }
            }
        }
    }

    let center_score = if s[1][1].is_finite() {
        s[1][1]
    } else {
        best.score
    };
    let (x_ref, y_ref) = refine_subpixel_2d(best.x, best.y, s);

    Ok(Match {
        x: x_ref,
        y: y_ref,
        angle_deg: 0.0,
        score: center_score,
    })
}