import cv2
import numpy as np

def angle_cos(p0, p1, p2):
    d1, d2 = (p0-p1).astype('float'), (p2-p1).astype('float')
    return abs( np.dot(d1, d2) / np.sqrt( np.dot(d1, d1)*np.dot(d2, d2) ) )

def find_squares(img):
    squares = []
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    # cv2.imshow("gray", gray)

    gaussian = cv2.GaussianBlur(gray, (5, 5), 0)

    thresh = cv2.adaptiveThreshold(gaussian, 255, 1, 1, 11, 2)
    #temp,bin = cv2.threshold(gaussian, 80, 255, cv2.THRESH_BINARY)
    cv2.imshow("thresh", thresh)

    contours, hierarchy = cv2.findContours(thresh, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)

    cv2.drawContours( gray, contours, -1, (0, 255, 0), 3 )

    #cv2.imshow('contours', gray)
    for cnt in contours:
        cnt_len = cv2.arcLength(cnt, True)
        cnt = cv2.approxPolyDP(cnt, 0.02*cnt_len, True)
        if cv2.contourArea(cnt) > 1000 :
            #cnt = cnt.reshape(-1, 2)
            #max_cos = np.max([angle_cos( cnt[i], cnt[(i+1) % 4], cnt[(i+2) % 4] ) for i in range(4)])
            #if max_cos < 0.1:
            squares.append(cnt)
    return squares

if __name__ == '__main__':
    img = cv2.imread('equip/main_screen.jpg')

    #cv2.imshow("origin", img)

    squares = find_squares(img)
    print("Find %d squres" % len(squares))
    cv2.drawContours( img, squares, -1, (0, 255, 0), 3 )
    cv2.imshow('squares', img)

    cv2.waitKey()