#!/usr/bin/env python
# -*- coding: utf-8 -*-
# @Time    : 2024/6/4 0004 下午 1:52
# @Author  : liudan
# @File    : image-stitching.py
# @Software: pycharm

import cv2 as cv
import cv2
import numpy as np
from loguru import logger
import os

from superpoint_superglue_deployment import Matcher

def registration_demo():
    query_image = cv2.imread('./data/stitcher_image/frame_000240.jpg') #hw
    ref_image = cv2.imread('./data/stitcher_image/frame_000330.jpg')

    new_size = (1420, 800) # W,H
    query_image_resize = cv2.resize(query_image,new_size)
    ref_image_resize = cv2.resize(ref_image,new_size)

    query_gray = cv2.cvtColor(query_image_resize, cv2.COLOR_BGR2GRAY)
    ref_gray = cv2.cvtColor(ref_image_resize, cv2.COLOR_BGR2GRAY)

    superglue_matcher = Matcher(
        {
            "superpoint": {
                "input_shape": (-1, -1),
                "keypoint_threshold": 0.003,
            },
            "superglue": {
                "match_threshold": 0.5,
            },
            "use_gpu": True,
        }
    )
    query_kpts, ref_kpts, _, _, matches = superglue_matcher.match(query_gray, ref_gray)
    M, mask = cv2.findHomography(
        np.float64([query_kpts[m.queryIdx].pt for m in matches]).reshape(-1, 1, 2),
        np.float64([ref_kpts[m.trainIdx].pt for m in matches]).reshape(-1, 1, 2),
        method=cv2.USAC_MAGSAC,
        ransacReprojThreshold=5.0,
        maxIters=10000,
        confidence=0.95,
    )
    logger.info(f"number of inliers: {mask.sum()}")
    matches = np.array(matches)[np.all(mask > 0, axis=1)]
    matches = sorted(matches, key=lambda match: match.distance)
    matched_image = cv2.drawMatches(
        query_image_resize,
        query_kpts,
        ref_image_resize,
        ref_kpts,
        matches[:50],
        None,
        flags=2,
    )



    wrap_image = cv.warpPerspective(query_image_resize, M, (ref_image_resize.shape[1] + query_image_resize.shape[1], ref_image_resize.shape[0]))
    wrap_image = cv2.resize(wrap_image,(ref_image.shape[1]+query_image.shape[1], ref_image.shape[0]))
    # wrap_filename = f"wrap_image_{idx1}_{idx2}_{iteration}.jpg"
    # cv2.imwrite(os.path.join(demo_image_path, wrap_filename), wrap_image)
    # cv2.imwrite(os.path.join(demo_image_path + f"wrap_image.jpg"), wrap_image)

    wrap_image[0:ref_image.shape[0], 0: ref_image.shape[1]] =ref_image
    cv2.imwrite("image2.jpg", wrap_image)


    return wrap_image



if __name__ == "__main__":
    registration_demo()