import cv2 
import numpy as np
import matplotlib.pyplot as plt
import pprint
import math

f = open("transforms.trf", "w")
f.write("VID.STAB 1
")
f.write("#      accuracy = 15
")
f.write("#     shakiness = 3
")
f.write("#      stepsize = 4
")
f.write("#   mincontrast = 0.200000
")
f.write("Frame 1 (List 0 [])
")

img1 = cv2.imread('DSC_4877_FerryHg.jpg')  
mask = cv2.imread('mask2.png', cv2.IMREAD_GRAYSCALE)
Mask_5182 = cv2.imread('Mask_5182.png', cv2.IMREAD_GRAYSCALE)
mask_5725 = cv2.imread('mask_5725.png', cv2.IMREAD_GRAYSCALE)
mask_5885 = cv2.imread('mask_5885.png', cv2.IMREAD_GRAYSCALE)
mask_6670 = cv2.imread('mask_6670.png', cv2.IMREAD_GRAYSCALE)


currentMask = mask

img1 = cv2.cvtColor(img1, cv2.COLOR_BGR2GRAY)

#sift
sift = cv2.SIFT_create()

keypoints_1, descriptors_1 = sift.detectAndCompute(img1,mask)

for n in range(2,2064):
    print("Frame " + str(n))
    img2 = cv2.imread("DSC_{:04d}_FerryHg.jpg".format(4876 + n)) 
    img2 = cv2.cvtColor(img2, cv2.COLOR_BGR2GRAY)

    if ( (4876 + n) == 5182 ):
        currentMask = Mask_5182

    if ( (4876 + n) == 5725 ):
        currentMask = mask_5725
    
    if ( (4876 + n) == 5885 ):
        currentMask = mask_5885

    if ( (4876 + n) == 6670 ):
        currentMask = mask_6670

    keypoints_2, descriptors_2 = sift.detectAndCompute(img2,currentMask)

    #feature matching
    bf = cv2.BFMatcher(cv2.NORM_L1, crossCheck=True)

    matches = bf.match(descriptors_1,descriptors_2)
    matches = sorted(matches, key = lambda x:x.distance)

    countPositions = 0
    listOfPoints = ""
    for idx, val in enumerate(matches):
        distance = math.sqrt( (keypoints_2[val.trainIdx].pt[0]-keypoints_1[val.queryIdx].pt[0]) *
                         (keypoints_2[val.trainIdx].pt[0]-keypoints_1[val.queryIdx].pt[0]) + 
                        ( keypoints_2[val.trainIdx].pt[1]-keypoints_1[val.queryIdx].pt[1]) *
                         (keypoints_2[val.trainIdx].pt[1]-keypoints_1[val.queryIdx].pt[1] ) )

        if ( val.distance > 299 and idx > 10 ):
            continue
    
        print("    " + str(idx) + " " + str(distance) + " " + str(val.distance) +" (" +
                 str(keypoints_1[val.queryIdx].pt[0]) + "," + str(keypoints_1[val.queryIdx].pt[1]) +
                 ") -> (" + str(keypoints_2[val.trainIdx].pt[0]) + "," +
                 str(keypoints_2[val.trainIdx].pt[1]) + ")" )
        if( countPositions > 0 ):
            listOfPoints = listOfPoints + ","

        # https://github.com/georgmartius/vid.stab/blob/master/src/serialize.c
        #   if(fscanf(f,"(LM %hi %hi %hi %hi %hi %lf %lf", &lm.v.x,&lm.v.y,&lm.f.x,&lm.f.y,&lm.f.size,
        #    &lm.contrast, &lm.match) != 7) {
        # LM = Local Motion
        # Sample: (LM 0 0 922 424 224 0.507735 0.263512)

        listOfPoints = listOfPoints + "(LM {:0.0f} {:0.0f} {:0.0f} {:0.0f} {:0.0f} {:5.3f} {:5.3f})".format(
                -(keypoints_2[val.trainIdx].pt[0]-keypoints_1[val.queryIdx].pt[0]),
                -(keypoints_2[val.trainIdx].pt[1]-keypoints_1[val.queryIdx].pt[1]),
                keypoints_1[val.queryIdx].pt[0], keypoints_1[val.queryIdx].pt[1], 
                48,
                (300.0 - val.distance) / 300.0 , 0.6 - (300.0 - val.distance) / 1000.0)
        countPositions = countPositions + 1


    f.write("Frame {:d} (List {:d} [{}])".format(n,countPositions,listOfPoints))
    img1 = img2
    keypoints_1 = keypoints_2
    descriptors_1 = descriptors_2


f.close()