import cv2
import numpy as np

from mower.utils import typealias as tp
from mower.utils.image import cropimg
from mower.utils.log import logger
from mower.utils.vector import va, vs

GOOD_DISTANCE_LIMIT = 0.8

ORB = cv2.ORB_create(nfeatures=1000000, edgeThreshold=0)
ORB_no_pyramid = cv2.ORB_create(nfeatures=1000000, edgeThreshold=0, nlevels=1)


def keypoints_scale_invariant(img: tp.GrayImage, mask: tp.GrayImage | None = None):
    return ORB.detectAndCompute(img, mask)


def keypoints(img: tp.GrayImage, mask: tp.GrayImage | None = None):
    return ORB_no_pyramid.detectAndCompute(img, mask)


FLANN_INDEX_LSH = 6
index_params = dict(
    algorithm=FLANN_INDEX_LSH,
    table_number=1,
    key_size=18,
    multi_probe_level=0,
)
search_params = dict(checks=50)
flann = cv2.FlannBasedMatcher(index_params, search_params)


def getHash(data: list[float]) -> tp.Hash:
    """calc image hash"""
    avreage = np.mean(data)
    return np.where(data > avreage, 1, 0)


def hammingDistance(hash1: tp.Hash, hash2: tp.Hash) -> int:
    """calc Hamming distance between two hash"""
    return np.count_nonzero(hash1 != hash2)


def aHash(img1: tp.GrayImage, img2: tp.GrayImage) -> int:
    """calc image hash"""
    data1 = cv2.resize(img1, (8, 4)).flatten()
    data2 = cv2.resize(img2, (8, 4)).flatten()
    hash1 = getHash(data1)
    hash2 = getHash(data2)
    return hammingDistance(hash1, hash2)


class Matcher:
    def __init__(self, origin: tp.GrayImage) -> None:
        logger.debug(f"{origin.shape=}")
        self.origin = origin
        self.kp, self.des = keypoints(self.origin)

    def in_scope(self, scope):
        if scope is None:
            return self.kp, self.des
        ori_kp, ori_des = [], []
        for _kp, _des in zip(self.kp, self.des):
            if (
                scope[0][0] <= _kp.pt[0] <= scope[1][0]
                and scope[0][1] <= _kp.pt[1] <= scope[1][1]
            ):
                ori_kp.append(_kp)
                ori_des.append(_des)
        logger.debug(f"{scope=}, {len(self.kp)=} -> {len(ori_kp)=}")
        return np.array(ori_kp), np.array(ori_des)

    def match(
        self,
        query: tp.GrayImage,
        draw: bool = False,
        scope: tp.Scope | None = None,
    ) -> tuple[float, tp.Scope | None]:
        if self.des is None:
            logger.debug(f"{self.des=}")
            return -1, None
        ori_kp, ori_des = self.in_scope(scope)
        if len(ori_kp) < 2:
            logger.debug(f"{len(ori_kp)=} < 2")
            return -1, None

        qry_kp, qry_des = keypoints(query)
        matches = flann.knnMatch(qry_des, ori_des, k=2)
        good = []
        for pair in matches:
            if (len_pair := len(pair)) == 2:
                x, y = pair
                if x.distance < GOOD_DISTANCE_LIMIT * y.distance:
                    good.append(x)
            elif len_pair == 1:
                good.append(pair[0])

        if draw:
            from matplotlib import pyplot as plt

            result = cv2.drawMatches(query, qry_kp, self.origin, ori_kp, good, None)
            plt.imshow(result)
            plt.show()

        if len(good) <= 4:
            logger.debug(f"{len(good)=} <= 4, {len(qry_des)=}")
            return -1, None

        qry_pts = np.float32([qry_kp[m.queryIdx].pt for m in good]).reshape(-1, 1, 2)
        ori_pts = np.float32([ori_kp[m.trainIdx].pt for m in good]).reshape(-1, 1, 2)
        M, mask = cv2.estimateAffine2D(qry_pts, ori_pts, None, cv2.RANSAC)

        if M is None:
            logger.debug("M is None")
            return -1, None
        else:
            logger.debug(f"M={M.tolist()}")

        M[0][1] = M[1][0] = 0
        M[0][0] = M[1][1] = 1

        h, w = query.shape

        if draw:
            matches_mask = mask.ravel().tolist()
            pts = np.int32([[[0, 0]], [[w - 1, h - 1]]])
            dst = np.int32(cv2.transform(pts, M)).tolist()
            dst = [i[0] for i in dst]
            dst = np.int32(
                [
                    [dst[0]],
                    [[dst[0][0], dst[1][1]]],
                    [dst[1]],
                    [[dst[1][0], dst[0][1]]],
                ]
            )
            disp = cv2.cvtColor(self.origin, cv2.COLOR_GRAY2RGB)
            disp = cv2.polylines(disp, [dst], True, (255, 0, 0), 2, cv2.LINE_AA)
            disp = cv2.drawMatches(
                query,
                qry_kp,
                disp,
                ori_kp,
                good,
                None,
                (0, 255, 0),
                matchesMask=matches_mask,
                flags=cv2.DrawMatchesFlags_NOT_DRAW_SINGLE_POINTS,
            )
            plt.imshow(disp)
            plt.show()

        pts = np.int32([[[-20, -20]], [[w + 19, h + 19]]])
        dst = cv2.transform(pts, M)
        rect = dst.reshape(2, 2).tolist()
        rect = np.array(rect, dtype=int).tolist()
        disp = cropimg(self.origin, rect)
        rh, rw = disp.shape
        if rh < h or rw < w:
            logger.debug(f"{rect=} {rh=} {h=} {rw=} {w=}")
            return -1, None

        result = cv2.matchTemplate(disp, query, cv2.TM_CCOEFF_NORMED)
        min_val, max_val, min_loc, max_loc = cv2.minMaxLoc(result)

        if draw:
            disp = cropimg(disp, (max_loc, va(max_loc, (w, h))))
            plt.subplot(1, 2, 1)
            plt.imshow(query, cmap="gray", vmin=0, vmax=255)
            plt.subplot(1, 2, 2)
            plt.imshow(disp, cmap="gray", vmin=0, vmax=255)
            plt.show()

        top_left = va(rect[0], max_loc)
        scope = top_left, va(top_left, (w, h))
        logger.debug(f"{max_val=} {scope=}")
        return max_val, scope

    def match2d(
        self,
        query: tp.GrayImage,
        draw: bool = False,
        scope: tp.Scope | None = None,
    ) -> tuple[float, tp.Scope | None]:
        if self.des is None:
            logger.debug(f"{self.des=}")
            return -1, None
        ori_kp, ori_des = self.in_scope(scope)
        if len(ori_kp) < 2:
            logger.debug(f"{len(ori_kp)=} < 2")
            return -1, None

        qry_kp, qry_des = keypoints_scale_invariant(query)
        matches = flann.knnMatch(qry_des, ori_des, k=2)
        good = []
        for pair in matches:
            if (len_pair := len(pair)) == 2:
                x, y = pair
                if x.distance < GOOD_DISTANCE_LIMIT * y.distance:
                    good.append(x)
            elif len_pair == 1:
                good.append(pair[0])

        if draw:
            from matplotlib import pyplot as plt

            result = cv2.drawMatches(query, qry_kp, self.origin, ori_kp, good, None)
            plt.imshow(result)
            plt.show()

        if len(good) <= 4:
            logger.debug(f"{len(good)=} <= 4, {len(qry_des)=}")
            return -1, None

        qry_pts = np.float32([qry_kp[m.queryIdx].pt for m in good]).reshape(-1, 1, 2)
        ori_pts = np.float32([ori_kp[m.trainIdx].pt for m in good]).reshape(-1, 1, 2)
        M, mask = cv2.estimateAffine2D(qry_pts, ori_pts, None, cv2.RANSAC)

        if M is None:
            logger.debug("M is None")
            return -1, None
        else:
            logger.debug(f"M={M.tolist()}")

        M[0][1] = M[1][0] = 0
        M[0][0] = M[1][1] = scale = (M[0][0] + M[1][1]) / 2

        logger.debug(f"{scale=}")

        h, w = query.shape

        if draw:
            matches_mask = mask.ravel().tolist()
            pts = np.int32([[[0, 0]], [[w - 1, h - 1]]])
            dst = np.int32(cv2.transform(pts, M)).tolist()
            dst = [i[0] for i in dst]
            dst = np.int32(
                [
                    [dst[0]],
                    [[dst[0][0], dst[1][1]]],
                    [dst[1]],
                    [[dst[1][0], dst[0][1]]],
                ]
            )
            disp = cv2.cvtColor(self.origin, cv2.COLOR_GRAY2RGB)
            disp = cv2.polylines(disp, [dst], True, (255, 0, 0), 2, cv2.LINE_AA)
            disp = cv2.drawMatches(
                query,
                qry_kp,
                disp,
                ori_kp,
                good,
                None,
                (0, 255, 0),
                matchesMask=matches_mask,
                flags=cv2.DrawMatchesFlags_NOT_DRAW_SINGLE_POINTS,
            )
            plt.imshow(disp)
            plt.show()

        pts = np.int32([[[-20, -20]], [[w + 19, h + 19]]])
        dst = cv2.transform(pts, M)
        rect = dst.reshape(2, 2).tolist()
        rect = np.array(rect, dtype=int).tolist()

        rect_img = cropimg(self.origin, rect)
        rh, rw = rect_img.shape
        if rh <= 0 or rw <= 0:
            logger.debug(f"{rh=} {rw=}")
            return -1, None

        disp = cv2.resize(rect_img, dsize=None, fx=1 / scale, fy=1 / scale)
        dh, dw = disp.shape
        if dh < h or dw < w:
            logger.debug(f"{dw=} {dh=} {w=} {h=}")
            return -1, None

        result = cv2.matchTemplate(disp, query, cv2.TM_CCOEFF_NORMED)
        min_val, max_val, min_loc, max_loc = cv2.minMaxLoc(result)

        if draw:
            disp = cropimg(disp, (max_loc, va(max_loc, (w, h))))
            plt.subplot(1, 2, 1)
            plt.imshow(query, cmap="gray", vmin=0, vmax=255)
            plt.subplot(1, 2, 2)
            plt.imshow(disp, cmap="gray", vmin=0, vmax=255)
            plt.show()

        top_left = vs(max_loc, (20, 20))
        scope = top_left, va(top_left, (w, h))
        scope = np.float32(scope).reshape(-1, 1, 2)
        scope = cv2.transform(scope, M)
        scope = np.int32(scope).reshape(2, 2).tolist()
        logger.debug(f"{max_val=} {scope=}")
        return max_val, scope

    def match3d(
        self,
        query: tp.GrayImage,
        draw: bool = False,
        scope: tp.Scope | None = None,
    ) -> tuple[float, tp.Scope | None]:
        if self.des is None:
            logger.debug(f"{self.des=}")
            return -1, None
        ori_kp, ori_des = self.in_scope(scope)
        if len(ori_kp) < 2:
            logger.debug(f"{len(ori_kp)=} < 2")
            return -1, None

        qry_kp, qry_des = keypoints_scale_invariant(query)
        matches = flann.knnMatch(qry_des, ori_des, k=2)
        good = []
        for pair in matches:
            if (len_pair := len(pair)) == 2:
                x, y = pair
                if x.distance < GOOD_DISTANCE_LIMIT * y.distance:
                    good.append(x)
            elif len_pair == 1:
                good.append(pair[0])

        if draw:
            from matplotlib import pyplot as plt

            result = cv2.drawMatches(query, qry_kp, self.origin, ori_kp, good, None)
            plt.imshow(result)
            plt.show()

        if len(good) <= 4:
            logger.debug(f"{len(good)=} <= 4, {len(qry_des)=}")
            return -1, None

        qry_pts = np.float32([qry_kp[m.queryIdx].pt for m in good]).reshape(-1, 1, 2)
        ori_pts = np.float32([ori_kp[m.trainIdx].pt for m in good]).reshape(-1, 1, 2)
        M, mask = cv2.findHomography(qry_pts, ori_pts, cv2.RANSAC)

        if M is None:
            logger.debug("M is None")
            return -1, None
        else:
            logger.debug(f"M={M.tolist()}")

        h, w = query.shape

        if draw:
            matches_mask = mask.ravel().tolist()
            pts = np.float32(
                [
                    [0, 0],
                    [0, h - 1],
                    [w - 1, h - 1],
                    [w - 1, 0],
                ]
            ).reshape(-1, 1, 2)
            dst = cv2.perspectiveTransform(pts, M)
            disp = cv2.cvtColor(self.origin, cv2.COLOR_GRAY2RGB)
            disp = cv2.polylines(
                disp, [np.int32(dst)], True, (255, 0, 0), 2, cv2.LINE_AA
            )
            disp = cv2.drawMatches(
                query,
                qry_kp,
                disp,
                ori_kp,
                good,
                None,
                (0, 255, 0),
                matchesMask=matches_mask,
                flags=cv2.DrawMatchesFlags_NOT_DRAW_SINGLE_POINTS,
            )
            plt.imshow(disp)
            plt.show()

        offset = (20, 20)
        A = np.array(
            [
                [1, 0, -offset[0]],
                [0, 1, -offset[1]],
                [0, 0, 1],
            ]
        )

        disp = cv2.warpPerspective(
            self.origin, M.dot(A), va((w, h), (40, 40)), None, cv2.WARP_INVERSE_MAP
        )
        result = cv2.matchTemplate(disp, query, cv2.TM_CCOEFF_NORMED)
        min_val, max_val, min_loc, max_loc = cv2.minMaxLoc(result)

        if draw:
            disp = cropimg(disp, (max_loc, va(max_loc, (w, h))))
            plt.subplot(1, 2, 1)
            plt.imshow(query, cmap="gray", vmin=0, vmax=255)
            plt.subplot(1, 2, 2)
            plt.imshow(disp, cmap="gray", vmin=0, vmax=255)
            plt.show()

        top_left = vs(max_loc, offset)
        scope = top_left, va(top_left, (w, h))
        scope = np.float32(scope).reshape(-1, 1, 2)
        scope = cv2.perspectiveTransform(scope, M)
        scope = np.int32(scope).reshape(2, 2).tolist()
        logger.debug(f"{max_val=} {scope=}")
        return max_val, scope
