# -*- coding: utf-8 -*-
import numpy as np
import cv2
from appenimage import appendimage
def hough_estimate_mistake(im1,im2,pts1,pts2):
    A=np.zeros((4,4,12,5))
    Apoint=np.zeros((len(pts1),12,5,4))
    point1=np.zeros(pts1.shape)
    point2=np.zeros(pts2.shape)
    for i in range(len(pts1)):
        for sita_index in range(12):
            sita=np.deg2rad(2*np.pi/12*sita_index)
            for scale_index in range(5):
                scale=(2**scale_index)*0.25
                xc=pts1[i,0]-(pts2[i,0]*np.cos(sita)-pts2[i,1]*np.sin(sita))*scale
                yc=pts1[i,1]-(pts2[i,0]*np.sin(sita)+pts2[i,1]*np.cos(sita))*scale
                x,y=0,0
                if xc<=0.25*im1.shape[1]:
                    x=0
                elif xc<=0.5*im1.shape[1]:
                    x=1
                elif xc<=0.75*im1.shape[1]:
                    x=2
                elif xc<=im1.shape[1]:
                    x=3
                    
                if yc<=0.25*im1.shape[0]:
                    y=0
                elif yc<=0.5*im1.shape[0]:
                    y=1
                elif yc<=0.75*im1.shape[0]:
                    y=2
                elif yc<=im1.shape[0]:
                    y=3
                if x>=0 and x<=3 and y>=0 and y<=3:
                    A[x,y,sita_index,scale_index]+=1
                    Apoint[i,sita_index,scale_index,:]=[x,y,sita_index,scale_index]
    max1=0
    for x in range(4):
        for y in range(4):
            tmpA=np.reshape(A[x,y,:,:],(12,5))
            tmp=np.max(tmpA) 
            if tmp>max1:
                max1=tmp
                locate=[x,y]
                sita,scale=np.where(tmpA==tmp)
                sita_scale=[sita[0],scale[0]]
    inner=0
    for i in range(len(pts1)):
        for sita_index in range(12):
            for scale_index in range(5):  
                x=Apoint[i,sita_index,scale_index,0]
                y=Apoint[i,sita_index,scale_index,1]
                sita_tmp=Apoint[i,sita_index,scale_index,2]
                scale_tmp=Apoint[i,sita_index,scale_index,3]
                if x==locate[0] and y==locate[1] and sita_tmp==sita_scale[0] and scale_tmp==sita_scale[1]:
                    point1[inner,:]=pts1[i,:]
                    point2[inner,:]=pts2[i,:]
                    inner+=1
    return point1,point2
def matchIMG(im1,im2,kp1,kp2,des1,des2):
        FLANN_INDEX_KDTREE=0
        index_p=dict(algorithm = FLANN_INDEX_KDTREE, trees = 5)
        searth_p=dict(checks=50)
        flann=cv2.FlannBasedMatcher(index_p,searth_p)
        matches=flann.knnMatch(des1,des2,k=2)
        good =[]
        pts1=[]
        pts2=[]
        for i,(m,n) in enumerate(matches):
            if m.distance<0.6*n.distance:
                good.append(m)
                pts1.append(kp1[m.queryIdx].pt)
                pts2.append(kp2[m.trainIdx].pt)
        pts1=np.float32(pts1)
        pts2=np.float32(pts2)
        return pts1,pts2     
if __name__=="__main__":
    im1_=cv2.imread(r"C:\Users\Y\Desktop\input_0.png")
    im2_=cv2.imread(r"C:\Users\Y\Desktop\input_1.png")
    im1=cv2.cvtColor(im1_,cv2.COLOR_BGR2GRAY)
    im2=cv2.cvtColor(im2_,cv2.COLOR_BGR2GRAY)
    im2=cv2.GaussianBlur(im2,(7,7),2)
    sift=cv2.xfeatures2d.SIFT_create()
    kp1,des1=sift.detectAndCompute(im1,None)
    kp2,des2=sift.detectAndCompute(im2,None)
    pts1,pts2=matchIMG(im1,im2,kp1,kp2,des1,des2)       
    point1,point2=np.float32(hough_estimate_mistake(im1,im2,pts1,pts2))
    im3=appendimage(im1,im2)
    pts2_new=pts2.copy()
    point2_new=point2.copy()
    for i in range(len(pts2)):
        pts2_new[i,0]=pts2_new[i,0]+np.float32(im1.shape[1])
    for i in range(len(pts2)):
        point2_new[i,0]=point2_new[i,0]+np.float32(im1.shape[1])
    for i in range(len(pts1)):
        cv2.line(im3,tuple(pts1[i]),tuple(pts2_new[i]),(0,255,0),2)
#    for i in range(len(point1)):
#        cv2.line(im3,tuple(point1[i]),tuple(point2_new[i]),(0,0,255),2)