import cv2
from cv2 import *
import numpy as np
from matplotlib import pyplot as plt




###############################SIFT MATCH Function#################################
def SIFTMATCH(img1,img2):

    # Initiate SIFT detector
    sift = cv2.xfeatures2d.SIFT_create()

    # find the keypoints and descriptors with SIFT
    kp1, des1 = sift.detectAndCompute(img1,None)
    kp2, des2 = sift.detectAndCompute(img2,None)

    FLANN_INDEX_KDTREE = 0
    index_params = dict(algorithm = FLANN_INDEX_KDTREE, trees = 5)
    search_params = dict(checks = 50)

    flann = cv2.FlannBasedMatcher(index_params, search_params)

    matches = flann.knnMatch(des1,des2,k=2)

    # store all the good matches as per Lowe's ratio test.
    good = []
    for m,n in matches:
        if m.distance < 0.7*n.distance:
            good.append(m)


    if len(good)>MIN_MATCH_COUNT:
        src_pts = np.float32([ kp1[m.queryIdx].pt for m in good ]).reshape(-1,1,2)
        dst_pts = np.float32([ kp2[m.trainIdx].pt for m in good ]).reshape(-1,1,2)

        M, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC,5.0)
        matchesMask = mask.ravel().tolist()

        h,w = img1.shape
        pts = np.float32([ [0,0],[0,h-1],[w-1,h-1],[w-1,0] ]).reshape(-1,1,2)
        dst = cv2.perspectiveTransform(pts,M)

        img2 = cv2.polylines(img2,[np.int32(dst)],True,255,3, cv2.LINE_AA)

    else:
        print("Not enough matches are found - %d/%d" % (len(good),MIN_MATCH_COUNT))
        matchesMask = None
        
        
    draw_params = dict(matchColor = (0,255,0), # draw matches in green color
                       singlePointColor = None,
                       matchesMask = matchesMask, # draw only inliers
                       flags = 2)

    img3 = cv2.drawMatches(img1,kp1,img2,kp2,good,None,**draw_params)


    cv2.moveWindow('output', 150,150)  # Move it to (40,30)
    cv2.imshow('output',img3)
    cv2.waitKey(0)			  #The function waits for specified milliseconds for any keyboard event
    cv2.destroyAllWindows()   #cv2.destroyAllWindows() simply destroys all the windows we created

###################################################################################################

#################################Function#########################

def CercleDetection(img1):
    # Read Image
    raw_image = cv2.imread(img1)

    # Bilateral filtering forms a very good way to preserve edges. It is a non-linear filter and helps reduce noise 
    # The parameters used are: the image, window size for averaging the neighbour, sigmaColor(Sigma value in the color space. 
    bilateral_filtered_image = cv2.bilateralFilter(raw_image, 5, 175, 175)

    # Canny edge detector to detect edges in the image It takes 3 parameters: image, lower threshold and upper threshold.
    edge_detected_image = cv2.Canny(bilateral_filtered_image, 75, 200)


    # Find Contours
    _, contours, hierarchy = cv2.findContours(edge_detected_image, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)

    contour_list = []
    for contour in contours:
        approx = cv2.approxPolyDP(contour,0.01*cv2.arcLength(contour,True),True)
        area = cv2.contourArea(contour)
        if ((len(approx) > 8) & (len(approx) < 23) & (area > 50000) ):
            contour_list.append(contour)
            print("area %.3f"%(area))
            M = cv2.moments(contour)
            # calculate x,y coordinate of center
            if M["m00"] != 0:
             cX = int(M["m10"] / M["m00"])
             cY = int(M["m01"] / M["m00"])
            else:
             cX, cY = 0, 0
            cv2.circle(raw_image, (cX, cY), 5, (255, 255, 255), -1)
            cv2.putText(raw_image, "centroid", (cX - 25, cY - 25),cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2)


            
            
    # Draw Contours of circles
    cv2.drawContours(raw_image, contour_list, -1, (0, 255, 0), 3)
   
    # Display Images
    cv2.imshow("Objects Detected",raw_image)
    cv2.waitKey(0)
    cv2.destroyAllWindows()
    return cX,cY

############################################################
    
###########################MAIN#############################   
    
MIN_MATCH_COUNT = 10
e1 = cv2.getTickCount()


# # initialize the camera
# cam = VideoCapture(0)   # 0 -> index of camera
# s, img1 = cam.read()
# ret = cam.set(3,1920);
# ret = cam.set(4,1080);

# if s:    # frame captured without any errors
    # cv2.namedWindow("output", cv2.WINDOW_NORMAL)
    # cv2.imshow("cam-test",img1)
    # waitKey(0)
    # destroyWindow("cam-test")
    # imwrite("Scene.jpg",img1) #save image
# del(cam)



# Scene image in Grayscale
# imgray = cv2.cvtColor(img1,cv2.COLOR_BGR2GRAY)
imgray = cv2.imread('Scene.jpg', 0) # queryImage


# Reference Piece Image
img1 = cv2.imread('img3.jpg',0) # queryImage
    
# SIFT Algorithm fore Object Detection
SIFTMATCH(img1, imgray)


# image de reference
cX, cY = CercleDetection('img3.jpg')
print('cX = %.3f , cY =%.3f' % (cX, cY))



# Image Webcam

cX2, cY2 = CercleDetection('img3.jpg')
print('cX2 = %.3f , cY2 =%.3f' % (cX2, cY2))


deltaX = (cX2-cX)
deltaY = -(CY2-cY)


# Write X and Y values to File
file = open("values.txt", "w")
file.write("%.3f \n" % deltaX)
file.write("%.3f \n" % deltaY)
file.close() 






#Calculate time of execution
e2 = cv2.getTickCount()
time = (e2 - e1)/ cv2.getTickFrequency()
print('time needed to execute')
print(time)






[((118, 5, 118, 23), 'cv2.getTickCount', 'cv2.getTickCount', ({}, {}), '()', False, 'import cv2\n'), ((139, 9, 139, 35), 'cv2.imread', 'cv2.imread', ({(139, 20, 139, 31): '"""Scene.jpg"""', (139, 33, 139, 34): '0'}, {}), "('Scene.jpg', 0)", False, 'import cv2\n'), ((143, 7, 143, 31), 'cv2.imread', 'cv2.imread', ({(143, 18, 143, 28): '"""img3.jpg"""', (143, 29, 143, 30): '0'}, {}), "('img3.jpg', 0)", False, 'import cv2\n'), ((177, 5, 177, 23), 'cv2.getTickCount', 'cv2.getTickCount', ({}, {}), '()', False, 'import cv2\n'), ((13, 11, 13, 40), 'cv2.xfeatures2d.SIFT_create', 'cv2.xfeatures2d.SIFT_create', ({}, {}), '()', False, 'import cv2\n'), ((23, 12, 23, 62), 'cv2.FlannBasedMatcher', 'cv2.FlannBasedMatcher', ({(23, 34, 23, 46): 'index_params', (23, 48, 23, 61): 'search_params'}, {}), '(index_params, search_params)', False, 'import cv2\n'), ((57, 11, 57, 69), 'cv2.drawMatches', 'cv2.drawMatches', ({(57, 27, 57, 31): 'img1', (57, 32, 57, 35): 'kp1', (57, 36, 57, 40): 'img2', (57, 41, 57, 44): 'kp2', (57, 45, 57, 49): 'good', (57, 50, 57, 54): 'None'}, {}), '(img1, kp1, img2, kp2, good, None, **draw_params)', False, 'import cv2\n'), ((60, 4, 60, 37), 'cv2.moveWindow', 'cv2.moveWindow', ({(60, 19, 60, 27): '"""output"""', (60, 29, 60, 32): '(150)', (60, 33, 60, 36): '(150)'}, {}), "('output', 150, 150)", False, 'import cv2\n'), ((61, 4, 61, 29), 'cv2.imshow', 'cv2.imshow', ({(61, 15, 61, 23): '"""output"""', (61, 24, 61, 28): 'img3'}, {}), "('output', img3)", False, 'import cv2\n'), ((62, 4, 62, 18), 'cv2.waitKey', 'cv2.waitKey', ({(62, 16, 62, 17): '(0)'}, {}), '(0)', False, 'import cv2\n'), ((63, 4, 63, 27), 'cv2.destroyAllWindows', 'cv2.destroyAllWindows', ({}, {}), '()', False, 'import cv2\n'), ((71, 16, 71, 32), 'cv2.imread', 'cv2.imread', ({(71, 27, 71, 31): 'img1'}, {}), '(img1)', False, 'import cv2\n'), ((75, 31, 75, 74), 'cv2.bilateralFilter', 'cv2.bilateralFilter', ({(75, 51, 75, 60): 'raw_image', (75, 62, 75, 63): '5', (75, 65, 75, 68): '175', (75, 70, 75, 73): '175'}, {}), '(raw_image, 5, 175, 175)', False, 'import cv2\n'), ((78, 26, 78, 70), 'cv2.Canny', 'cv2.Canny', ({(78, 36, 78, 60): 'bilateral_filtered_image', (78, 62, 78, 64): '75', (78, 66, 78, 69): '200'}, {}), '(bilateral_filtered_image, 75, 200)', False, 'import cv2\n'), ((82, 29, 82, 106), 'cv2.findContours', 'cv2.findContours', ({(82, 46, 82, 65): 'edge_detected_image', (82, 67, 82, 80): 'cv2.RETR_TREE', (82, 82, 82, 105): 'cv2.CHAIN_APPROX_SIMPLE'}, {}), '(edge_detected_image, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)', False, 'import cv2\n'), ((105, 4, 105, 65), 'cv2.drawContours', 'cv2.drawContours', ({(105, 21, 105, 30): 'raw_image', (105, 32, 105, 44): 'contour_list', (105, 46, 105, 48): '(-1)', (105, 50, 105, 61): '(0, 255, 0)', (105, 63, 105, 64): '(3)'}, {}), '(raw_image, contour_list, -1, (0, 255, 0), 3)', False, 'import cv2\n'), ((108, 4, 108, 44), 'cv2.imshow', 'cv2.imshow', ({(108, 15, 108, 33): '"""Objects Detected"""', (108, 34, 108, 43): 'raw_image'}, {}), "('Objects Detected', raw_image)", False, 'import cv2\n'), ((109, 4, 109, 18), 'cv2.waitKey', 'cv2.waitKey', ({(109, 16, 109, 17): '(0)'}, {}), '(0)', False, 'import cv2\n'), ((110, 4, 110, 27), 'cv2.destroyAllWindows', 'cv2.destroyAllWindows', ({}, {}), '()', False, 'import cv2\n'), ((178, 18, 178, 40), 'cv2.getTickFrequency', 'cv2.getTickFrequency', ({}, {}), '()', False, 'import cv2\n'), ((38, 18, 38, 70), 'cv2.findHomography', 'cv2.findHomography', ({(38, 37, 38, 44): 'src_pts', (38, 46, 38, 53): 'dst_pts', (38, 55, 38, 65): 'cv2.RANSAC', (38, 66, 38, 69): '5.0'}, {}), '(src_pts, dst_pts, cv2.RANSAC, 5.0)', False, 'import cv2\n'), ((43, 14, 43, 45), 'cv2.perspectiveTransform', 'cv2.perspectiveTransform', ({(43, 39, 43, 42): 'pts', (43, 43, 43, 44): 'M'}, {}), '(pts, M)', False, 'import cv2\n'), ((87, 15, 87, 39), 'cv2.contourArea', 'cv2.contourArea', ({(87, 31, 87, 38): 'contour'}, {}), '(contour)', False, 'import cv2\n'), ((91, 16, 91, 36), 'cv2.moments', 'cv2.moments', ({(91, 28, 91, 35): 'contour'}, {}), '(contour)', False, 'import cv2\n'), ((98, 12, 98, 67), 'cv2.circle', 'cv2.circle', ({(98, 23, 98, 32): 'raw_image', (98, 34, 98, 42): '(cX, cY)', (98, 44, 98, 45): '(5)', (98, 47, 98, 62): '(255, 255, 255)', (98, 64, 98, 66): '(-1)'}, {}), '(raw_image, (cX, cY), 5, (255, 255, 255), -1)', False, 'import cv2\n'), ((99, 12, 99, 116), 'cv2.putText', 'cv2.putText', ({(99, 24, 99, 33): 'raw_image', (99, 35, 99, 45): '"""centroid"""', (99, 47, 99, 65): '(cX - 25, cY - 25)', (99, 66, 99, 90): 'cv2.FONT_HERSHEY_SIMPLEX', (99, 92, 99, 95): '(0.5)', (99, 97, 99, 112): '(255, 255, 255)', (99, 114, 99, 115): '(2)'}, {}), "(raw_image, 'centroid', (cX - 25, cY - 25), cv2.\n    FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2)", False, 'import cv2\n'), ((35, 18, 35, 66), 'numpy.float32', 'np.float32', ({(35, 29, 35, 65): '[kp1[m.queryIdx].pt for m in good]'}, {}), '([kp1[m.queryIdx].pt for m in good])', True, 'import numpy as np\n'), ((36, 18, 36, 66), 'numpy.float32', 'np.float32', ({(36, 29, 36, 65): '[kp2[m.trainIdx].pt for m in good]'}, {}), '([kp2[m.trainIdx].pt for m in good])', True, 'import numpy as np\n'), ((42, 14, 42, 61), 'numpy.float32', 'np.float32', ({(42, 25, 42, 60): '[[0, 0], [0, h - 1], [w - 1, h - 1], [w - 1, 0]]'}, {}), '([[0, 0], [0, h - 1], [w - 1, h - 1], [w - 1, 0]])', True, 'import numpy as np\n'), ((45, 35, 45, 48), 'numpy.int32', 'np.int32', ({(45, 44, 45, 47): 'dst'}, {}), '(dst)', True, 'import numpy as np\n'), ((86, 47, 86, 74), 'cv2.arcLength', 'cv2.arcLength', ({(86, 61, 86, 68): 'contour', (86, 69, 86, 73): 'True'}, {}), '(contour, True)', False, 'import cv2\n')]