"""
MatchTool - Python port of MatchToolDlg.cpp
Template matching with rotation support, image pyramid, sub-pixel estimation.

This is the core algorithm only (no BaseModel/opt.json wiring) - see
ecos_core.match_tool.match_tool.Model for the ECOS-integrated wrapper.
"""

import cv2
import numpy as np
import math
import time
from dataclasses import dataclass, field
from typing import List, Tuple, Optional

# ─── Constants ────────────────────────────────────────────────────────────────
VISION_TOLERANCE = 1e-7
D2R = math.pi / 180.0
R2D = 180.0 / math.pi
MATCH_CANDIDATE_NUM = 5

# ─── Data Structures ─────────────────────────────────────────────────────────

@dataclass
class TemplData:
    """Stores learned template data (pyramid, statistics, etc.)."""
    vec_pyramid: List[np.ndarray] = field(default_factory=list)
    vec_inv_area: List[float] = field(default_factory=list)
    vec_templ_mean: List[np.ndarray] = field(default_factory=list)
    vec_templ_norm: List[float] = field(default_factory=list)
    vec_result_equal1: List[bool] = field(default_factory=list)
    i_border_color: int = 0
    b_is_pattern_learned: bool = False

    def resize(self, size: int):
        self.vec_inv_area = [0.0] * size
        self.vec_templ_mean = [np.zeros(4)] * size
        self.vec_templ_norm = [0.0] * size
        self.vec_result_equal1 = [False] * size


@dataclass
class MatchParameter:
    """Stores a single match candidate during the matching process."""
    pt: np.ndarray = field(default_factory=lambda: np.array([0.0, 0.0]))
    d_match_score: float = 0.0
    d_match_angle: float = 0.0
    d_angle_start: float = 0.0
    d_angle_end: float = 0.0
    b_delete: bool = False
    b_pos_on_border: bool = False
    rect_r: Optional[tuple] = None  # RotatedRect as ((cx,cy),(w,h),angle)
    # 3x3 result grid for sub-pixel estimation
    vec_result: np.ndarray = field(default_factory=lambda: np.zeros((3, 3)))


@dataclass
class SingleTargetMatch:
    """Stores final match result for one target."""
    pt_lt: np.ndarray = field(default_factory=lambda: np.array([0.0, 0.0]))
    pt_rt: np.ndarray = field(default_factory=lambda: np.array([0.0, 0.0]))
    pt_rb: np.ndarray = field(default_factory=lambda: np.array([0.0, 0.0]))
    pt_lb: np.ndarray = field(default_factory=lambda: np.array([0.0, 0.0]))
    pt_center: np.ndarray = field(default_factory=lambda: np.array([0.0, 0.0]))
    d_matched_angle: float = 0.0
    d_match_score: float = 0.0


@dataclass
class BlockMax:
    """Divides the result matrix into blocks to speed up max-location search."""
    mat_result: np.ndarray = None
    block_size: Tuple[int, int] = (1, 1)
    block_max_vals: np.ndarray = None
    block_max_locs: List[Tuple[int, int]] = field(default_factory=list)
    n_blocks_x: int = 0
    n_blocks_y: int = 0

    def __init__(self, mat_result: np.ndarray, size_template: Tuple[int, int]):
        self.mat_result = mat_result
        self.block_size = size_template  # (width, height)
        h, w = mat_result.shape[:2]
        bw, bh = size_template
        self.n_blocks_x = max(1, w // bw)
        self.n_blocks_y = max(1, h // bh)
        self._compute_blocks()

    def _compute_blocks(self):
        h, w = self.mat_result.shape[:2]
        bw = self.block_size[0]
        bh = self.block_size[1]
        self.block_max_vals = np.full((self.n_blocks_y, self.n_blocks_x), -np.inf)
        self.block_max_locs = [
            [None] * self.n_blocks_x for _ in range(self.n_blocks_y)
        ]
        for by in range(self.n_blocks_y):
            for bx in range(self.n_blocks_x):
                y0 = by * bh
                y1 = min(y0 + bh, h)
                x0 = bx * bw
                x1 = min(x0 + bw, w)
                block = self.mat_result[y0:y1, x0:x1]
                if block.size == 0:
                    continue
                local_max_idx = np.unravel_index(np.argmax(block), block.shape)
                self.block_max_vals[by, bx] = block[local_max_idx]
                self.block_max_locs[by][bx] = (x0 + local_max_idx[1], y0 + local_max_idx[0])

    def get_max_value_loc(self) -> Tuple[float, Tuple[int, int]]:
        idx = np.unravel_index(np.argmax(self.block_max_vals), self.block_max_vals.shape)
        max_val = self.block_max_vals[idx]
        max_loc = self.block_max_locs[idx[0]][idx[1]]
        return float(max_val), max_loc

    def update_max(self, rect: Tuple[int, int, int, int]):
        """Re-compute blocks that overlap with the zeroed-out rectangle."""
        rx, ry, rw, rh = rect
        h, w = self.mat_result.shape[:2]
        bw, bh = self.block_size
        bx_start = max(0, rx // bw)
        bx_end = min(self.n_blocks_x, (rx + rw) // bw + 1)
        by_start = max(0, ry // bh)
        by_end = min(self.n_blocks_y, (ry + rh) // bh + 1)
        for by in range(by_start, by_end):
            for bx in range(bx_start, bx_end):
                y0 = by * bh
                y1 = min(y0 + bh, h)
                x0 = bx * bw
                x1 = min(x0 + bw, w)
                block = self.mat_result[y0:y1, x0:x1]
                if block.size == 0:
                    continue
                local_max_idx = np.unravel_index(np.argmax(block), block.shape)
                self.block_max_vals[by, bx] = block[local_max_idx]
                self.block_max_locs[by][bx] = (x0 + local_max_idx[1], y0 + local_max_idx[0])


# ─── Core Algorithm Class ────────────────────────────────────────────────────

class MatchTool:
    def __init__(
        self,
        max_pos: int = 70,
        max_overlap: float = 0.0,
        score: float = 0.5,
        tolerance_angle: float = 0.0,
        min_reduce_area: int = 256,
        debug_mode: bool = False,
        use_subpixel: bool = False,

        stop_layer1: bool = False,
        tolerance_range: bool = False,
        tolerance1: float = 40.0,
        tolerance2: float = 60.0,
        tolerance3: float = -110.0,
        tolerance4: float = -100.0,
        crop_xyxy: Optional[Tuple[int, int, int, int]] = None,
    ):
        self.max_pos = max_pos
        self.max_overlap = max_overlap
        self.score = score
        self.tolerance_angle = tolerance_angle
        self.min_reduce_area = min_reduce_area
        self.debug_mode = debug_mode
        self.use_subpixel = use_subpixel
        self.stop_layer1 = stop_layer1
        self.tolerance_range = tolerance_range
        self.tolerance1 = tolerance1
        self.tolerance2 = tolerance2
        self.tolerance3 = tolerance3
        self.tolerance4 = tolerance4
        self.crop_xyxy = crop_xyxy
        self.templ_data = TemplData()
        self.mat_src: Optional[np.ndarray] = None
        self.mat_dst: Optional[np.ndarray] = None
        self.results: List[SingleTargetMatch] = []

    # ── Image I/O ─────────────────────────────────────────────────────────

    def load_source(self, source: str | np.ndarray):
        if isinstance(source, str):
            self.mat_src = cv2.imread(source, cv2.IMREAD_GRAYSCALE)
        else:
            self.mat_src = source
        x1, y1, x2, y2 = self.crop_xyxy if self.crop_xyxy else (0, 0, self.mat_src.shape[1], self.mat_src.shape[0])
        self.mat_src = self.mat_src[y1:y2, x1:x2]
        if self.mat_src is None:
            raise FileNotFoundError(f"Cannot read source image")


    def load_template(self, path: str | np.ndarray):
        if isinstance(path, str):
            self.mat_dst = cv2.imread(path, cv2.IMREAD_GRAYSCALE)
        else:
            self.mat_dst = path

        if self.mat_dst is None:
            raise FileNotFoundError(f"Cannot read template image")
        self.learn_pattern()

    # ── Pattern Learning ──────────────────────────────────────────────────

    def learn_pattern(self):
        """Pre-compute pyramid and statistics for the template."""
        self.templ_data = TemplData()
        mat_dst = self.mat_dst
        top_layer = self._get_top_layer(mat_dst, int(math.sqrt(self.min_reduce_area)))

        # Build Gaussian pyramid
        pyramid = [mat_dst.copy()]
        tmp = mat_dst.copy()
        for _ in range(top_layer):
            tmp = cv2.pyrDown(tmp)
            pyramid.append(tmp)
        self.templ_data.vec_pyramid = pyramid

        # Border color
        self.templ_data.i_border_color = 255 if np.mean(mat_dst) < 128 else 0

        n_levels = len(pyramid)
        self.templ_data.resize(n_levels)

        for i in range(n_levels):
            h, w = pyramid[i].shape[:2]
            inv_area = 1.0 / (h * w)
            mean_val, std_val = cv2.meanStdDev(pyramid[i])
            templ_norm = float(np.sum(std_val ** 2))

            if templ_norm < np.finfo(float).eps:
                self.templ_data.vec_result_equal1[i] = True

            templ_sum2 = templ_norm + float(np.sum(mean_val ** 2))
            templ_sum2 /= inv_area
            templ_norm = math.sqrt(templ_norm)
            templ_norm /= math.sqrt(inv_area)

            self.templ_data.vec_inv_area[i] = inv_area
            self.templ_data.vec_templ_mean[i] = mean_val.flatten()
            self.templ_data.vec_templ_norm[i] = templ_norm

        self.templ_data.b_is_pattern_learned = True

    # ── Helpers ────────────────────────────────────────────────────────────

    @staticmethod
    def _get_top_layer(mat_templ: np.ndarray, min_dst_length: int) -> int:
        top_layer = 0
        min_area = min_dst_length * min_dst_length
        h, w = mat_templ.shape[:2]
        area = w * h
        while area > min_area:
            area //= 4
            top_layer += 1
        return top_layer

    @staticmethod
    def _pt_rotate_pt2f(pt_input: np.ndarray, pt_org: np.ndarray, angle: float) -> np.ndarray:
        """Rotate pt_input around pt_org by angle (radians). Matches C++ ptRotatePt2f."""
        d_height = pt_org[1] * 2
        dy1 = d_height - pt_input[1]
        dx = (pt_input[0] - pt_org[0]) * math.cos(angle) - (dy1 - pt_org[1]) * math.sin(angle) + pt_org[0]
        dy = (pt_input[0] - pt_org[0]) * math.sin(angle) + (dy1 - pt_org[1]) * math.cos(angle) + (d_height - pt_org[1])
        dy = -dy + d_height
        return np.array([dx, dy], dtype=np.float64)

    @staticmethod
    def _get_best_rotation_size(
        size_src: Tuple[int, int],
        size_dst: Tuple[int, int],
        d_r_angle: float,
    ) -> Tuple[int, int]:
        """Compute best canvas size for a rotated image. Returns (width, height)."""
        sw, sh = size_src
        dw, dh = size_dst
        angle_rad = d_r_angle * D2R
        pt_center = np.array([(sw - 1) / 2.0, (sh - 1) / 2.0])

        corners = [
            np.array([0.0, 0.0]),
            np.array([0.0, sh - 1.0]),
            np.array([sw - 1.0, sh - 1.0]),
            np.array([sw - 1.0, 0.0]),
        ]
        rotated = [MatchTool._pt_rotate_pt2f(c, pt_center, angle_rad) for c in corners]
        xs = [r[0] for r in rotated]
        ys = [r[1] for r in rotated]

        f_top_y = max(ys)
        f_bottom_y = min(ys)
        f_right_x = max(xs)
        f_left_x = min(xs)

        angle = d_r_angle
        if angle > 360:
            angle -= 360
        elif angle < 0:
            angle += 360

        if abs(abs(angle) - 90) < VISION_TOLERANCE or abs(abs(angle) - 270) < VISION_TOLERANCE:
            return (sh, sw)
        if abs(angle) < VISION_TOLERANCE or abs(abs(angle) - 180) < VISION_TOLERANCE:
            return (sw, sh)

        # Normalize angle to 0-90
        a = angle
        if 0 < a < 90:
            pass
        elif 90 < a < 180:
            a -= 90
        elif 180 < a < 270:
            a -= 180
        elif 270 < a < 360:
            a -= 270

        fh1 = dw * math.sin(a * D2R) * math.cos(a * D2R)
        fh2 = dh * math.sin(a * D2R) * math.cos(a * D2R)

        half_height = int(math.ceil(f_top_y - pt_center[1] - fh1))
        half_width = int(math.ceil(f_right_x - pt_center[0] - fh2))
        ret_w, ret_h = half_width * 2, half_height * 2

        wrong = (dw < ret_w and dh > ret_h) or (dw > ret_w and dh < ret_h) or (dw * dh > ret_w * ret_h)
        if wrong:
            ret_w = int(f_right_x - f_left_x + 0.5)
            ret_h = int(f_top_y - f_bottom_y + 0.5)

        return (ret_w, ret_h)

    # ── CCOEFF Denominator (NCC normalization) ────────────────────────────

    def _ccoeff_denominator(
        self, mat_src: np.ndarray, templ_data: TemplData, mat_result: np.ndarray, i_layer: int
    ) -> np.ndarray:
        """Normalize cross-correlation to get CCOEFF_NORMED values."""
        if templ_data.vec_result_equal1[i_layer]:
            return np.ones_like(mat_result)

        templ_h, templ_w = templ_data.vec_pyramid[i_layer].shape[:2]
        d_templ_mean0 = templ_data.vec_templ_mean[i_layer][0]
        d_templ_norm = templ_data.vec_templ_norm[i_layer]
        d_inv_area = templ_data.vec_inv_area[i_layer]

        # Integral images
        src_f64 = mat_src.astype(np.float64)
        sum_img = cv2.integral(src_f64)  # shape (H+1, W+1)
        sqsum_img = cv2.integral(src_f64, sdepth=cv2.CV_64F)
        # cv2.integral returns (sum, sqsum) when called differently; use separate calls
        sqsum_img = cv2.integral(src_f64 * src_f64)

        rh, rw = mat_result.shape[:2]
        result = mat_result.copy()

        for i in range(rh):
            for j in range(rw):
                # Sum of window
                t = (
                    sum_img[i, j]
                    - sum_img[i, j + templ_w]
                    - sum_img[i + templ_h, j]
                    + sum_img[i + templ_h, j + templ_w]
                )
                wnd_mean2 = t * t * d_inv_area
                num = float(result[i, j]) - t * d_templ_mean0

                # Sum of squares of window
                t2 = (
                    sqsum_img[i, j]
                    - sqsum_img[i, j + templ_w]
                    - sqsum_img[i + templ_h, j]
                    + sqsum_img[i + templ_h, j + templ_w]
                )
                wnd_sum2 = t2

                diff2 = max(wnd_sum2 - wnd_mean2, 0)
                if diff2 <= min(0.5, 10 * np.finfo(np.float32).eps * wnd_sum2):
                    denom = 0.0
                else:
                    denom = math.sqrt(diff2) * d_templ_norm

                if denom != 0 and abs(num) < denom:
                    result[i, j] = num / denom
                elif denom != 0 and abs(num) < denom * 1.125:
                    result[i, j] = 1.0 if num > 0 else -1.0
                else:
                    result[i, j] = 0.0

        return result

    def _match_template(
        self,
        mat_src: np.ndarray,
        templ_data: TemplData,
        i_layer: int,
    ) -> np.ndarray:
        """Cross-correlation + CCOEFF normalization."""
        templ = templ_data.vec_pyramid[i_layer]
        if mat_src.shape[0] < templ.shape[0] or mat_src.shape[1] < templ.shape[1]:
            # Source smaller than template — return a 1x1 result with score -1
            return np.full((1, 1), -1.0, dtype=np.float64)
        mat_result = cv2.matchTemplate(
            mat_src, templ_data.vec_pyramid[i_layer], cv2.TM_CCORR
        )
        mat_result = self._ccoeff_denominator(mat_src, templ_data, mat_result, i_layer)
        return mat_result

    # ── Rotated ROI ───────────────────────────────────────────────────────

    def _get_rotated_roi(
        self,
        mat_src: np.ndarray,
        size: Tuple[int, int],
        pt_lt: np.ndarray,
        d_angle: float,
    ) -> np.ndarray:
        """Extract a rotated ROI from mat_src."""
        h, w = mat_src.shape[:2]
        pt_c = np.array([(w - 1) / 2.0, (h - 1) / 2.0])
        angle_rad = d_angle * D2R
        pt_lt_rot = self._pt_rotate_pt2f(pt_lt, pt_c, angle_rad)
        size_padding = (size[0] + 6, size[1] + 6)

        r_mat = cv2.getRotationMatrix2D(tuple(pt_c), d_angle, 1.0)
        r_mat[0, 2] -= pt_lt_rot[0] - 3
        r_mat[1, 2] -= pt_lt_rot[1] - 3
        mat_roi = cv2.warpAffine(mat_src, r_mat, size_padding)
        return mat_roi

    # ── Next Max Location ─────────────────────────────────────────────────

    @staticmethod
    def _get_next_max_loc(
        mat_result: np.ndarray,
        pt_max_loc: Tuple[int, int],
        size_template: Tuple[int, int],
        max_overlap: float,
        block_max: Optional[BlockMax] = None,
    ) -> Tuple[Tuple[int, int], float]:
        tw, th = size_template
        start_x = int(pt_max_loc[0] - tw * (1 - max_overlap))
        start_y = int(pt_max_loc[1] - th * (1 - max_overlap))
        rect_w = int(2 * tw * (1 - max_overlap))
        rect_h = int(2 * th * (1 - max_overlap))

        # Clamp
        rh, rw = mat_result.shape[:2]
        x0 = max(0, start_x)
        y0 = max(0, start_y)
        x1 = min(rw, start_x + rect_w)
        y1 = min(rh, start_y + rect_h)
        mat_result[y0:y1, x0:x1] = -1.0

        if block_max is not None:
            block_max.update_max((x0, y0, x1 - x0, y1 - y0))
            d_max_val, pt_new = block_max.get_max_value_loc()
            return pt_new, d_max_val
        else:
            _, d_max_val, _, pt_new = cv2.minMaxLoc(mat_result)
            return pt_new, d_max_val

    # ── Sub-Pixel Estimation ──────────────────────────────────────────────

    @staticmethod
    def _sub_pix_estimation(
        vec: List[MatchParameter],
        angle_step: float,
        max_score_idx: int,
    ) -> Tuple[float, float, float]:
        """Least-squares quadratic sub-pixel refinement."""
        mat_a = np.zeros((27, 10), dtype=np.float64)
        mat_s = np.zeros((27, 1), dtype=np.float64)

        x_max = vec[max_score_idx].pt[0]
        y_max = vec[max_score_idx].pt[1]
        theta_max = vec[max_score_idx].d_match_angle

        row = 0
        for theta in range(3):  # 0, 1, 2
            for y in range(-1, 2):
                for x in range(-1, 2):
                    dx = x_max + x
                    dy = y_max + y
                    dt = (theta_max + (theta - 1) * angle_step) * D2R
                    mat_a[row] = [
                        dx * dx, dy * dy, dt * dt,
                        dx * dy, dx * dt, dy * dt,
                        dx, dy, dt, 1.0,
                    ]
                    mat_s[row, 0] = vec[max_score_idx + (theta - 1)].vec_result[x + 1, y + 1]
                    row += 1

        # z = (A^T A)^-1 A^T s
        ata = mat_a.T @ mat_a
        ats = mat_a.T @ mat_s
        try:
            mat_z = np.linalg.solve(ata, ats).flatten()
        except np.linalg.LinAlgError:
            return x_max, y_max, theta_max

        # Solve for extremum: K1 * delta = K2
        k1 = np.array([
            [2 * mat_z[0], mat_z[3], mat_z[4]],
            [mat_z[3], 2 * mat_z[1], mat_z[5]],
            [mat_z[4], mat_z[5], 2 * mat_z[2]],
        ])
        k2 = np.array([-mat_z[6], -mat_z[7], -mat_z[8]])
        try:
            delta = np.linalg.solve(k1, k2)
        except np.linalg.LinAlgError:
            return x_max, y_max, theta_max

        return delta[0], delta[1], delta[2] * R2D

    # ── Overlap Filtering ─────────────────────────────────────────────────

    @staticmethod
    def _sort_pts_with_center(pts: List[np.ndarray]) -> List[np.ndarray]:
        """Sort intersection points by angle around their centroid."""
        center = np.mean(pts, axis=0)
        angles = []
        for p in pts:
            v = p - center
            norm_v = np.dot(v, v)
            if norm_v < 1e-12:
                angles.append(0.0)
                continue
            cos_a = v[0] / norm_v
            if v[1] < 0:
                angles.append(math.acos(np.clip(cos_a, -1, 1)) * R2D)
            elif v[1] > 0:
                angles.append(360 - math.acos(np.clip(cos_a, -1, 1)) * R2D)
            else:
                angles.append(0.0 if v[0] > 0 else 180.0)
        idx = sorted(range(len(pts)), key=lambda k: angles[k])
        return [pts[i] for i in idx]

    def _filter_with_score(self, vec: List[MatchParameter]) -> List[MatchParameter]:
        vec.sort(key=lambda m: m.d_match_score, reverse=True)
        cutoff = len(vec)
        for i, m in enumerate(vec):
            if m.d_match_score < self.score:
                cutoff = i
                break
        return vec[:cutoff]

    @staticmethod
    def _filter_with_rotated_rect(
        vec: List[MatchParameter], max_overlap: float
    ) -> List[MatchParameter]:
        n = len(vec)
        for i in range(n - 1):
            if vec[i].b_delete:
                continue
            for j in range(i + 1, n):
                if vec[j].b_delete:
                    continue
                r1 = vec[i].rect_r
                r2 = vec[j].rect_r
                if r1 is None or r2 is None:
                    continue
                ret, pts = cv2.rotatedRectangleIntersection(r1, r2)
                if ret == cv2.INTERSECT_NONE:
                    continue
                elif ret == cv2.INTERSECT_FULL:
                    delete_idx = j if vec[i].d_match_score >= vec[j].d_match_score else i
                    vec[delete_idx].b_delete = True
                else:
                    if pts is None or len(pts) < 3:
                        continue
                    pts_list = [pts[k][0] for k in range(len(pts))]
                    sorted_pts = MatchTool._sort_pts_with_center(pts_list)
                    area = cv2.contourArea(np.array(sorted_pts, dtype=np.float32))
                    r1_area = r1[1][0] * r1[1][1]
                    if r1_area > 0 and area / r1_area > max_overlap:
                        delete_idx = j if vec[i].d_match_score >= vec[j].d_match_score else i
                        vec[delete_idx].b_delete = True
        return [m for m in vec if not m.b_delete]

    # ── Main Match ────────────────────────────────────────────────────────

    def match(self) -> List[SingleTargetMatch]:
        """
        Run the full template matching pipeline.
        Returns list of SingleTargetMatch results.
        """
        if self.mat_src is None or self.mat_dst is None:
            raise ValueError("Source and template images must be loaded first.")
        if not self.templ_data.b_is_pattern_learned:
            raise ValueError("Pattern not learned. Call learn_pattern() first.")

        src_h, src_w = self.mat_src.shape[:2]
        dst_h, dst_w = self.mat_dst.shape[:2]

        # Validation checks. The width/height cross-inequality check only makes
        # sense when the template is searched axis-aligned: with rotation search
        # enabled, a template whose upright bounding box exceeds one source
        # dimension can still fit once the source is rotated into alignment
        # (Stage 1 rotates+pads the source canvas before matching).
        searches_rotation = self.tolerance_range or self.tolerance_angle >= VISION_TOLERANCE
        if not searches_rotation and (
            (dst_w < src_w and dst_h > src_h) or (dst_w > src_w and dst_h < src_h)
        ):
            return []
        if dst_w * dst_h > src_w * src_h:
            return []

        t_start = time.time()
        templ_data = self.templ_data

        top_layer = self._get_top_layer(self.mat_dst, int(math.sqrt(self.min_reduce_area)))

        # Build source pyramid

        src_pyramid = self._build_pyramid(self.mat_src, top_layer)

        # Build angle list
        d_angle_step = math.atan(2.0 / max(templ_data.vec_pyramid[top_layer].shape[:2])) * R2D

        vec_angles = []

        if self.tolerance_range:
            if self.tolerance1 >= self.tolerance2 or self.tolerance3 >= self.tolerance4:
                print("Error: Angle range setting abnormal, left value must be < right value")
                return []
            a = self.tolerance1
            while a < self.tolerance2 + d_angle_step:
                vec_angles.append(a)
                a += d_angle_step
            a = self.tolerance3
            while a < self.tolerance4 + d_angle_step:
                vec_angles.append(a)
                a += d_angle_step
        else:
            if self.tolerance_angle < VISION_TOLERANCE:
                vec_angles.append(0.0)
            else:
                a = 0.0
                while a < self.tolerance_angle + d_angle_step:
                    vec_angles.append(a)
                    a += d_angle_step
                a = -d_angle_step
                while a > -self.tolerance_angle - d_angle_step:
                    vec_angles.append(a)
                    a -= d_angle_step

        top_src_h, top_src_w = src_pyramid[top_layer].shape[:2]
        pt_center = np.array([(top_src_w - 1) / 2.0, (top_src_h - 1) / 2.0])

        # Layer score thresholds
        vec_layer_score = [self.score]
        for _ in range(top_layer):
            vec_layer_score.append(vec_layer_score[-1] * 0.9)

        size_pat = templ_data.vec_pyramid[top_layer].shape[:2]  # (h, w)
        pat_area = size_pat[0] * size_pat[1]
        src_top_area = src_pyramid[top_layer].shape[0] * src_pyramid[top_layer].shape[1]
        use_block_max = (src_top_area / pat_area > 500) and self.max_pos > 10

        vec_match_param: List[MatchParameter] = []

        # ── Stage 1: Top layer search ─────────────────────────────────────
        for angle in vec_angles:
            r_mat = cv2.getRotationMatrix2D(tuple(pt_center), angle, 1.0)
            size_best = self._get_best_rotation_size(
                (top_src_w, top_src_h),
                (size_pat[1], size_pat[0]),
                angle,
            )
            fx = (size_best[0] - 1) / 2.0 - pt_center[0]
            fy = (size_best[1] - 1) / 2.0 - pt_center[1]
            r_mat[0, 2] += fx
            r_mat[1, 2] += fy

            rotated_src = cv2.warpAffine(
                src_pyramid[top_layer], r_mat, size_best,
                flags=cv2.INTER_LINEAR,
                borderMode=cv2.BORDER_CONSTANT,
                borderValue=templ_data.i_border_color,
            )

            mat_result = self._match_template(rotated_src, templ_data, top_layer)

            if use_block_max:
                bm = BlockMax(mat_result, (size_pat[1], size_pat[0]))
                d_max_val, pt_max_loc = bm.get_max_value_loc()
                if d_max_val < vec_layer_score[top_layer]:
                    continue
                mp = MatchParameter()
                mp.pt = np.array([pt_max_loc[0] - fx, pt_max_loc[1] - fy])
                mp.d_match_score = d_max_val
                mp.d_match_angle = angle
                vec_match_param.append(mp)
                for _ in range(self.max_pos + MATCH_CANDIDATE_NUM - 1):
                    pt_max_loc, d_val = self._get_next_max_loc(
                        mat_result, pt_max_loc, (size_pat[1], size_pat[0]),
                        self.max_overlap, bm,
                    )
                    if d_val < vec_layer_score[top_layer]:
                        break
                    mp2 = MatchParameter()
                    mp2.pt = np.array([pt_max_loc[0] - fx, pt_max_loc[1] - fy])
                    mp2.d_match_score = d_val
                    mp2.d_match_angle = angle
                    vec_match_param.append(mp2)
            else:
                _, d_max_val, _, pt_max_loc = cv2.minMaxLoc(mat_result)
                if d_max_val < vec_layer_score[top_layer]:
                    continue
                mp = MatchParameter()
                mp.pt = np.array([pt_max_loc[0] - fx, pt_max_loc[1] - fy])
                mp.d_match_score = d_max_val
                mp.d_match_angle = angle
                vec_match_param.append(mp)
                for _ in range(self.max_pos + MATCH_CANDIDATE_NUM - 1):
                    pt_max_loc, d_val = self._get_next_max_loc(
                        mat_result, pt_max_loc, (size_pat[1], size_pat[0]),
                        self.max_overlap,
                    )
                    if d_val < vec_layer_score[top_layer]:
                        break
                    mp2 = MatchParameter()
                    mp2.pt = np.array([pt_max_loc[0] - fx, pt_max_loc[1] - fy])
                    mp2.d_match_score = d_val
                    mp2.d_match_angle = angle
                    vec_match_param.append(mp2)

        # Sort by score descending
        vec_match_param.sort(key=lambda m: m.d_match_score, reverse=True)

        dst_w_top = templ_data.vec_pyramid[top_layer].shape[1]
        dst_h_top = templ_data.vec_pyramid[top_layer].shape[0]

        # ── Stage 2: Refine through lower layers ──────────────────────────
        stop_layer = 1 if self.stop_layer1 else 0
        vec_all_result: List[MatchParameter] = []

        for i in range(len(vec_match_param)):
            d_r_angle = -vec_match_param[i].d_match_angle * D2R
            pt_lt = self._pt_rotate_pt2f(vec_match_param[i].pt, pt_center, d_r_angle)

            d_angle_step_local = math.atan(2.0 / max(dst_w_top, dst_h_top)) * R2D
            vec_match_param[i].d_angle_start = vec_match_param[i].d_match_angle - d_angle_step_local
            vec_match_param[i].d_angle_end = vec_match_param[i].d_match_angle + d_angle_step_local

            if top_layer <= stop_layer:
                vec_match_param[i].pt = pt_lt * (1 if top_layer == 0 else 2)
                vec_all_result.append(vec_match_param[i])
            else:
                for i_layer in range(top_layer - 1, stop_layer - 1, -1):
                    templ_h_l, templ_w_l = templ_data.vec_pyramid[i_layer].shape[:2]
                    d_angle_step_local = math.atan(2.0 / max(templ_w_l, templ_h_l)) * R2D

                    layer_angles = []
                    matched_angle = vec_match_param[i].d_match_angle
                    if self.tolerance_range:
                        for k in range(-1, 2):
                            layer_angles.append(matched_angle + d_angle_step_local * k)
                    else:
                        if self.tolerance_angle < VISION_TOLERANCE:
                            layer_angles.append(0.0)
                        else:
                            for k in range(-1, 2):
                                layer_angles.append(matched_angle + d_angle_step_local * k)

                    src_h_l, src_w_l = src_pyramid[i_layer].shape[:2]
                    pt_src_center = np.array([(src_w_l - 1) / 2.0, (src_h_l - 1) / 2.0])

                    n_angles = len(layer_angles)
                    new_params = [MatchParameter() for _ in range(n_angles)]
                    max_score_idx = 0
                    best_val = -1.0

                    for j in range(n_angles):
                        rotated_roi = self._get_rotated_roi(
                            src_pyramid[i_layer],
                            (templ_w_l, templ_h_l),
                            pt_lt * 2,
                            layer_angles[j],
                        )
                        mat_result = self._match_template(rotated_roi, templ_data, i_layer)
                        _, d_max_val, _, pt_max_loc = cv2.minMaxLoc(mat_result)

                        new_params[j].pt = np.array([pt_max_loc[0], pt_max_loc[1]], dtype=np.float64)
                        new_params[j].d_match_score = d_max_val
                        new_params[j].d_match_angle = layer_angles[j]

                        if d_max_val > best_val:
                            max_score_idx = j
                            best_val = d_max_val

                        # Check border for sub-pixel
                        rh_r, rw_r = mat_result.shape[:2]
                        if pt_max_loc[0] == 0 or pt_max_loc[1] == 0 or pt_max_loc[0] == rw_r - 1 or pt_max_loc[1] == rh_r - 1:
                            new_params[j].b_pos_on_border = True
                        if not new_params[j].b_pos_on_border:
                            for dy in range(-1, 2):
                                for dx in range(-1, 2):
                                    py = pt_max_loc[1] + dy
                                    px = pt_max_loc[0] + dx
                                    if 0 <= py < rh_r and 0 <= px < rw_r:
                                        new_params[j].vec_result[dx + 1, dy + 1] = mat_result[py, px]

                    if new_params[max_score_idx].d_match_score < vec_layer_score[i_layer]:
                        break

                    # Sub-pixel estimation
                    if (
                        self.use_subpixel
                        and i_layer == 0
                        and not new_params[max_score_idx].b_pos_on_border
                        and max_score_idx != 0
                        and max_score_idx != 2
                    ):
                        new_x, new_y, new_angle = self._sub_pix_estimation(
                            new_params, d_angle_step_local, max_score_idx
                        )
                        new_params[max_score_idx].pt = np.array([new_x, new_y])
                        new_params[max_score_idx].d_match_angle = new_angle

                    new_angle_val = new_params[max_score_idx].d_match_angle

                    # Transform coordinates back
                    pt_padding_lt = self._pt_rotate_pt2f(pt_lt * 2, pt_src_center, new_angle_val * D2R) - np.array([3.0, 3.0])
                    pt = new_params[max_score_idx].pt + pt_padding_lt
                    pt = self._pt_rotate_pt2f(pt, pt_src_center, -new_angle_val * D2R)

                    if i_layer == stop_layer:
                        new_params[max_score_idx].pt = pt * (1 if stop_layer == 0 else 2)
                        vec_all_result.append(new_params[max_score_idx])
                    else:
                        vec_match_param[i].d_match_angle = new_angle_val
                        vec_match_param[i].d_angle_start = new_angle_val - d_angle_step_local / 2
                        vec_match_param[i].d_angle_end = new_angle_val + d_angle_step_local / 2
                        pt_lt = pt

        # ── Stage 3: Filter results ───────────────────────────────────────
        vec_all_result = self._filter_with_score(vec_all_result)

        # Compute RotatedRect for overlap filtering
        iw_stop = templ_data.vec_pyramid[stop_layer].shape[1] * (1 if stop_layer == 0 else 2)
        ih_stop = templ_data.vec_pyramid[stop_layer].shape[0] * (1 if stop_layer == 0 else 2)

        for m in vec_all_result:
            dr = -m.d_match_angle * D2R
            pt_lt_m = m.pt
            pt_rt = np.array([pt_lt_m[0] + iw_stop * math.cos(dr), pt_lt_m[1] - iw_stop * math.sin(dr)])
            pt_lb = np.array([pt_lt_m[0] + ih_stop * math.sin(dr), pt_lt_m[1] + ih_stop * math.cos(dr)])
            pt_rb = np.array([pt_rt[0] + ih_stop * math.sin(dr), pt_rt[1] + ih_stop * math.cos(dr)])
            pts = np.array([pt_lt_m, pt_rt, pt_rb], dtype=np.float32)
            # Approximate RotatedRect from 3 points
            center = np.mean([pt_lt_m, pt_rt, pt_rb, pt_lb], axis=0)
            w = np.linalg.norm(pt_rt - pt_lt_m)
            h = np.linalg.norm(pt_lb - pt_lt_m)
            m.rect_r = (tuple(center), (float(w), float(h)), float(-m.d_match_angle))

        vec_all_result = self._filter_with_rotated_rect(vec_all_result, self.max_overlap)
        vec_all_result.sort(key=lambda m: m.d_match_score, reverse=True)

        # ── Build final results ───────────────────────────────────────────
        iw0 = templ_data.vec_pyramid[0].shape[1]
        ih0 = templ_data.vec_pyramid[0].shape[0]
        self.results = []

        for idx, m in enumerate(vec_all_result):
            dr = -m.d_match_angle * D2R
            sstm = SingleTargetMatch()
            sstm.pt_lt = m.pt.copy()
            sstm.pt_rt = np.array([sstm.pt_lt[0] + iw0 * math.cos(dr), sstm.pt_lt[1] - iw0 * math.sin(dr)])
            sstm.pt_lb = np.array([sstm.pt_lt[0] + ih0 * math.sin(dr), sstm.pt_lt[1] + ih0 * math.cos(dr)])
            sstm.pt_rb = np.array([sstm.pt_rt[0] + ih0 * math.sin(dr), sstm.pt_rt[1] + ih0 * math.cos(dr)])
            sstm.pt_center = (sstm.pt_lt + sstm.pt_rt + sstm.pt_rb + sstm.pt_lb) / 4.0
            sstm.d_matched_angle = -m.d_match_angle
            sstm.d_match_score = m.d_match_score

            if sstm.d_matched_angle < -180:
                sstm.d_matched_angle += 360
            if sstm.d_matched_angle > 180:
                sstm.d_matched_angle -= 360

            self.results.append(sstm)
            if idx + 1 >= self.max_pos:
                break

        elapsed = (time.time() - t_start) * 1000
        return self.results

    @staticmethod
    def _build_pyramid(img: np.ndarray, levels: int) -> List[np.ndarray]:
        pyramid = [img.copy()]
        tmp = img.copy()
        for _ in range(levels):
            tmp = cv2.pyrDown(tmp)
            pyramid.append(tmp)
        return pyramid

    # ── Visualization ─────────────────────────────────────────────────────

    def draw_results(self, mat_src: Optional[np.ndarray] = None) -> np.ndarray:
        """Draw match results on the source image and return the annotated image."""
        if mat_src is None:
            mat_src = self.mat_src
        if len(mat_src.shape) == 2:
            display = cv2.cvtColor(mat_src, cv2.COLOR_GRAY2BGR)
        else:
            display = mat_src.copy()

        green = (0, 255, 0)
        for i, r in enumerate(self.results):
            pts = np.array([r.pt_lt, r.pt_rt, r.pt_rb, r.pt_lb], dtype=np.int32)
            cv2.polylines(display, [pts], True, green, 1, cv2.LINE_AA)
            cx, cy = int(r.pt_center[0]), int(r.pt_center[1])
            cv2.drawMarker(display, (cx, cy), green, cv2.MARKER_CROSS, 10, 1)
            cv2.putText(display, str(i), tuple(((r.pt_lt + r.pt_rt) / 2).astype(int)),
                        cv2.FONT_HERSHEY_PLAIN, 1, green)
            # put angle
            cv2.putText(display, f"{r.d_matched_angle:.1f} deg ", tuple(((r.pt_lt + r.pt_lb) / 2).astype(int)),
                        cv2.FONT_HERSHEY_PLAIN, 1, (0, 0, 255))
        return display

    def print_results(self):
        print(f"\n{'Index':>5}  {'Score':>8}  {'Angle':>10}  {'PosX':>10}  {'PosY':>10}")
        print("-" * 50)
        for i, r in enumerate(self.results):
            print(f"{i:>5}  {r.d_match_score:>8.3f}  {r.d_matched_angle:>10.3f}  "
                  f"{r.pt_center[0]:>10.3f}  {r.pt_center[1]:>10.3f}")
