import matplotlib.pyplot as plt
import numpy as np
import cv2 as cv
import string

def saveto(filename):
    plt.savefig('LaTeX Report/figures/'+ filename)

def saveimg(filename, image):
    cv.imwrite('LaTeX Report/figures/'+ filename,image)

# points a, b and, c
a, b, c, d = (0, 0, 1), (0, 1, 1), (1, 1, 1), (1, 0, 1)
# matrix with row vectors of points
P = np.array([a, b, c, d]).T

# Translation
t = 1 # translation along each axis
H = [[1,0,t],[0,1,t],[0,0,1]]
Pt = np.matmul(H, P)

# Rotation 
theta = np.pi/4 # Anti clockwise pi/4 rad rotation
H = [[np.cos(theta), -np.sin(theta), 0.], [np.sin(theta), np.cos(theta), 0.], [0., 0., 1.]]
Pt = np.matmul(H, P)

# Rotation + translation.(2D Euclidean transformation)
H = [[np.cos(theta), -np.sin(theta), t], [np.sin(theta), np.cos(theta), t], [0., 0., 1.]]
Pt = np.matmul(H, P)

# Scaled rotation (similarity transform)
s = 0.5
H = [[s*np.cos(theta), -s*np.sin(theta), t], [s*np.sin(theta), s*np.cos(theta), t], [0., 0., 1.]]
Pt = np.matmul(H, P)

# Affine Parallel lines remain parallel under affine transformation
a00 = 3 ; a01 = 2 ; a02 = 3
a10 = 1 ; a11 = 3 ; a12 = 3
H = [[a00, a01, a02],[a10, a11, a12],[0, 0, 1]]
Pt = np.matmul(H, P)

# Projective(perspective transform or homography)
a20 = 1.5 ; a21 = 2; a22 = 1
H = [[a00, a01, a02],[a10, a11, a12],[a20,a21,a22]]
Pt = np.matmul(H, P)

#https://learnopencv.com/homography-examples-using-opencv-python-c/
import cv2 as cv
import numpy as np
N = 5
global n
n = 0
p1 = np.empty((N,2))
p2 = np.empty((N,2))
# mouse callback function
def draw_circle(event,x,y,flags,param):
    global n
    p = param[0]
    if event == cv.EVENT_LBUTTONDOWN:
        cv.circle(param[1],(x,y),5,(255,0,0),-1)
        p[n] = (x,y)
        n += 1
im1 = cv.imread('../images/graf/img1.ppm', cv.IMREAD_ANYCOLOR)
im4 = cv.imread('../images/graf/img4.ppm', cv.IMREAD_ANYCOLOR)

im1copy = im1.copy()
im4copy = im4.copy()
cv.namedWindow('Image 1', cv.WINDOW_AUTOSIZE)
param = [p1, im1copy]
cv.setMouseCallback('Image 1',draw_circle, param)
while(1):
    cv.imshow("Image 1", im1copy)
    if n == N:
        break
    if cv.waitKey(20) & 0xFF == 27:
        break
        
param = [p2, im4copy]
n = 0
cv.namedWindow("Image 4", cv.WINDOW_AUTOSIZE)
cv.setMouseCallback('Image 4',draw_circle, param)
while(1):
    cv.imshow("Image 4", im4copy)
    if n == N:
        break
    if cv.waitKey(20) & 0xFF == 27:
        break
cv.destroyAllWindows()

H, status = cv.findHomography(p1,p2)
H = np.array(H)
im4_warped = cv.warpPerspective(im4, np.linalg.inv(H), (900,900))
saveimg('im4warped.png', im4_warped)
im4_warped[0:im1.shape[0], 0:im1.shape[1]] = im1
saveimg('im4warped2.png', im4_warped)

#Marking the Selected points on the images
im1 = cv.imread('../images/graf/img1.ppm', cv.IMREAD_ANYCOLOR)
im4 = cv.imread('../images/graf/img4.ppm', cv.IMREAD_ANYCOLOR)

for point in p1.astype(np.int32):
    cv.circle(im1, tuple(point), radius=8, color=(255, 255, 0), thickness=-1)
saveimg('im1points.png', im1)
for point in p2.astype(np.int32):
    cv.circle(im4, tuple(point), radius=8, color=(255, 255, 0), thickness=-1)
saveimg('im4points.png', im4)

#============================ Normalization ======================
def normalizePoints(points):
    """
       Normalizing the point cloud(pre-conditioning) 
    """
    # Calculating the centroid of the points.
    centroid = sum(points)/len(points)
    # Calculating the scaling factor.
    total_dist = sum(np.sqrt(np.sum(((points - centroid)**2),axis = 1)))
    avg_dist  = total_dist/len(points)
    # For average distance from the origin to be sqrt(2) after scaling
    scale = np.sqrt(2)/avg_dist    
    # Defining Similarity transformation: translation and scaling
    xt, yt = centroid
    transform = np.array([[scale, 0, -xt*scale],
                          [0, scale, -yt*scale],
                          [0, 0,1]])
    # Making the points homogeneous by adding 1 at the end
    points = np.concatenate((points, np.ones((len(points),1))), axis =1)
    # Similarity transformation through matrix multiplication
    normalized_points = transform.dot(points.T).T
    return transform, normalized_points

#===================== Calculating homography ====================
def calcHomography(p1,p2):
    """
        The normalized DLT for 2D homographies.Given in the 
        Multiple View Geometry in Computer Vision Second Edition 
        by Richard Hartley & Andrew Zisserman. Algorithm 4.2 
    """
    # Normalizing the points using predefined function
    T1,p1 = normalizePoints(p1)
    T2,p2 = normalizePoints(p2)
    #Initialising an array to keep the coefficint matrix
    A = np.zeros((2*len(p1), 9))
    row = 0
    # Filling rows of the matrix according to the expressions
    for point1, point2 in zip(p1,p2):
        # Coefficients of the current row 
        A[row, 3:6] = -point2[2]*point1
        A[row, 6:9] =  point2[1]*point1
        # Coefficients of the next row 
        A[row+1, 0:3] =  point2[2]*point1
        A[row+1, 6:9] = -point2[0]*point1    
        row+=2    
    # Singular Value decomposition of A
    U, D, VT = np.linalg.svd(A)
    # unit singular vector corresponding to the smallest 
    # singular value, is the solution h. That is last column of V.
    # i.e. Last row of the V^T
    h =   VT[-1]
    # Reshaping to get 3x3 homography
    H = h.reshape((3,3))
    # Denormalization
    H = np.linalg.inv(T2).dot(H).dot(T1)
    H = H/H[-1,-1]
    return H

myH = calcHomography(p1,p2)
myH = np.array(myH)

im1 = cv.imread('../images/graf/img1.ppm', cv.IMREAD_ANYCOLOR)
im4 = cv.imread('../images/graf/img4.ppm', cv.IMREAD_ANYCOLOR)

im4_warped = cv.warpPerspective(im4, np.linalg.inv(myH), (900,900))
saveimg('myim4warped.png', im4_warped)
im4_warped[0:im1.shape[0], 0:im1.shape[1]] = im1
saveimg('myim4warped2.png', im4_warped)

for point in p1.astype(np.int32):
    cv.circle(im1, tuple(point), radius=8, color=(0, 255, 0), thickness=-1)
saveimg('myim1points.png', im1)
for point in p2.astype(np.int32):
    cv.circle(im4, tuple(point), radius=8, color=(0, 255, 0), thickness=-1)
saveimg('myim4points.png', im4)