from cv2_41 import cv2
import numpy as np

# rotate_bound: helper function that rotates the image adds some padding to avoid cutting off parts of it
# reference: https://www.pyimagesearch.com/2017/01/02/rotate-images-correctly-with-opencv-and-python/
def rotate_bound(image, angle):
    # grab the dimensions of the image and then determine the center
    (h, w) = image.shape[:2]
    (cX, cY) = (w // 2, h // 2)

    # grab the rotation matrix (applying the negative of the angle to rotate clockwise), then grab the sine and cosine
    # (i.e., the rotation components of the matrix)
    M = cv2.getRotationMatrix2D((cX, cY), -angle, 1.0)
    cos = np.abs(M[0, 0])
    sin = np.abs(M[0, 1])

    # compute the new bounding dimensions of the image
    nW = int(np.multiply(h, sin) + np.multiply(w, cos))
    nH = int(np.multiply(h, cos) + np.multiply(w, sin))

    # adjust the rotation matrix to take into account translation
    M[0, 2] += (nW / 2) - cX
    M[1, 2] += (nH / 2) - cY

    # perform rotation and return the image (white background) along with the Rotation Matrix
    return cv2.warpAffine(image, M, (nW, nH), borderValue=(255,255,255)), M


# Step 1 - Load images
input_img = cv2.imread("target.png", cv2.IMREAD_GRAYSCALE)
template_img = cv2.imread("template.png", cv2.IMREAD_GRAYSCALE)
matches_dbg_img = cv2.cvtColor(input_img, cv2.COLOR_GRAY2BGR) # for debugging purposes

# Step 2 - Generate some ROIs
# each ROI contains the x,y,w,h and angle (degree) to rotate the box and make its M appear horizontal
roi_w = 26
roi_h = 26

roi_list = []
roi_list.append((112, 7, roi_w, roi_h, 0))
roi_list.append((192, 36, roi_w, roi_h, -45))
roi_list.append((227, 104, roi_w, roi_h, -90))
roi_list.append((195, 183, roi_w, roi_h, -135))
roi_list.append((118, 216, roi_w, roi_h, -180))
roi_list.append((49, 196, roi_w, roi_h, -225))
roi_list.append((10, 114, roi_w, roi_h, -270))
roi_list.append((36, 41, roi_w, roi_h, -315))

# debug: draw green ROIs
rois_dbg_img = cv2.cvtColor(input_img, cv2.COLOR_GRAY2BGR)
for roi in roi_list:
    x, y, w, h, angle = roi
    x2 = x + w
    y2 = y + h
    cv2.rectangle(rois_dbg_img, (x, y), (x2, y2), (0,255,0), 2)

cv2.imwrite('target_rois.png', rois_dbg_img)
cv2.imshow('ROIs', rois_dbg_img)
cv2.waitKey(0)
cv2.destroyWindow('ROIs')


# Step 3 - Select a ROI, crop and rotate it, then perform Template Matching
for i, roi in enumerate(roi_list):
    x, y, w, h, angle = roi
    roi_cropped = input_img[y:y+h, x:x+w]
    roi_rotated, M = rotate_bound(roi_cropped, angle)

    # debug: display each rotated ROI
    #cv2.imshow('ROIs-cropped-rotated', roi_rotated)
    #cv2.waitKey(0)

    # debug: dump roi to the disk (before/after rotation)
    filename = 'target_roi' + str(i)
    cv2.imwrite(filename + '.png', roi_cropped)
    cv2.imwrite(filename + '_rotated.png', roi_rotated)

    # perform template matching
    res = cv2.matchTemplate(roi_rotated, template_img, cv2.TM_CCOEFF_NORMED)
    (_, score, _, (pos_x, pos_y)) = cv2.minMaxLoc(res)
    print('TM score=', score)

    # Step 4 - When a TM is found, revert the rotation of matched point so that it represents a location in the original image
    # Note: pos_x and pos_y define the location of the matched template in a rotated ROI
    threshold = 0.75
    if (score >= threshold):

        # debug in cropped image
        print('find_k_symbol: FOUND pos_x=', pos_x, 'pos_y=', pos_y, 'w=', template_img.shape[1], 'h=', template_img.shape[0])
        rot_output_roi = cv2.cvtColor(roi_rotated, cv2.COLOR_GRAY2BGR)
        cv2.rectangle(rot_output_roi, (pos_x, pos_y), (pos_x + template_img.shape[1], pos_y + template_img.shape[0]), (0, 165, 255), 2) # orange
        cv2.imshow('rot-matched-template', rot_output_roi)
        cv2.waitKey(0)
        cv2.destroyWindow('rot-matched-template')

        ###
        # How to convert the location of the matched template (pos_x, pos_y) to points in roi_cropped?
        # (which is the ROI before rotation)
        ###

        # extract variables from the rotation matrix
        M_x = M[0][2]
        M_y = M[1][2]
        #print('M_x=', M_x, '\tM_y=', M_y)
        M_cosx = M[0][0]
        M_msinx = M[0][1]
        #print('M_cosx=', M_cosx, '\tM_msinx=', M_msinx)
        M_siny = M[1][0]
        M_cosy = M[1][1]
        #print('M_siny=', M_siny, '\tM_cosy=', M_cosy)

        # undo translation:
        dst1_x = pos_x - M_x
        dst1_y = pos_y - M_y

        # undo rotation:
        # after this operation, (new_pos_x, new_pos_y) should already be a valid point in the original ROI
        new_pos_x =  M_cosx * dst1_x - M_msinx * dst1_y
        new_pos_y = -M_siny * dst1_x + M_cosy  * dst1_y

        # debug: create the bounding rect of the detected symbol in the original input image
        detected_x = x + int(new_pos_x)
        detected_y = y + int(new_pos_y)
        detected_w = template_img.shape[1]
        detected_h = template_img.shape[0]
        detected_rect = (detected_x, detected_y, detected_w, detected_h)

        print('find_k_symbol: detected_x=', detected_x, 'detected_y=', detected_y, 'detected_w=', detected_w, 'detected_h=', detected_h)
        print()

        bb_points = np.array([
            (detected_x, detected_y)
            , (detected_x + detected_w, detected_y)
            , (detected_x + detected_w, detected_y + detected_h)
            , (detected_x, detected_y + detected_h)
        ])
        Mrot = cv2.getRotationMatrix2D((detected_x, detected_y), angle, 1.0)

        rot_points = np.array([np.dot(Mrot, np.array([p[0], p[1], 1]).T) for p in bb_points], np.int32)
        cv2.polylines(matches_dbg_img, [rot_points], True, (0, 165, 255), 2)

        #cv2.rectangle(matches_dbg_img, (detected_x, detected_y), (detected_x + detected_w, detected_y + detected_h), (0, 165, 255), 2) # orange
        cv2.imwrite('target_matches.png', matches_dbg_img)
        cv2.imshow('matches', matches_dbg_img)
        cv2.waitKey(0)