import cv2
import numpy as np

def distanceBtwPoints(p0, p1):
    return np.sqrt((p0[0] - p1[0])**2 + (p0[1] - p1[1])**2)

def findNearestPointIndex(pt, Points):
    mindistance = 1e9

    for i in range(len(Points)):
#         print "Points i", Points[i]
#         print "pt", pt
        distance = distanceBtwPoints(pt, Points[i])
#         print 'distance', distance
#
        if distance < mindistance :
            mindistance =  distance
            nearestpointindex = i



    return  nearestpointindex

# def findNearestPointIndex(pt, Points):
#     mindistance = 1e9
#
#     for ip in Points:
#
# #         print "ip", ip
# #         print "pt", pt
#
#         distance = distanceBtwPoints(pt, ip)
#         print 'distance', distance
# #
#         if distance < mindistance :
#             mindistance =  distance
#             nearestpoint = ip
#             print "nearestpoint", nearestpoint
#             nearestpointindex = nearestpoint.argmin()
#             print "nearestpointindex", nearestpoint.argmin()
#
#
#     return  nearestpointindex

if __name__ == '__main__':
    img = cv2.imread("bridgeTestSource.png")
#     src = cv2.imread("bridgeTestSource.png")

    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    gray = np.asarray(gray < 127, dtype=np.uint8) # need to cast back to uint8


    contours, hierarchy = cv2.findContours(gray, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)

    cv2.drawContours(img, contours,-1, (0,0,0), 1)
    cnt = contours[0]

    for i in range(len(contours)):
        if len(cnt) < len(contours[i]):
            cnt = contours[i]
    for i in range(len(contours)):

        if not np.array_equal(cnt, contours[i]) and len(contours[i]) > 10:
            for j in range(len(contours[i])):
                pt0 = contours[i][j]

                cnt_nearIdx = cnt[findNearestPointIndex(pt0.squeeze(), cnt.squeeze())].squeeze()
                lcnt_nearIdx = cnt_nearIdx.tolist()
                lpt0 = (pt0.squeeze()).tolist()
                tcnt_nearIdx = tuple(lcnt_nearIdx)
                tpt0 = tuple(lpt0)
                cv2.line(img,tpt0, tcnt_nearIdx,(0,0,0),1,8)

    cv2.imwrite('nimg.jpg', img)

    gray_src = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    gray_src1 = np.asarray(gray_src < 127, dtype=np.uint8) # need to cast back to uint8


    contours1, hierarchy = cv2.findContours(gray_src1, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)

    imgMask = np.ones(img.shape)*255

    cv2.drawContours(imgMask, contours1,-1, 0, 0)

    m_xor= np.ones(imgMask.shape) * 255

    ncnt = cnt.astype(np.int32)

    cv2.polylines(m_xor, [ncnt], 1, (0,0,0))
    cv2.imwrite('resultpoly.jpg', m_xor)


    newImg = (cv2.bitwise_xor(imgMask, m_xor))

    invNewImg = cv2.bitwise_not(newImg)

    cv2.imwrite('result.jpg', newImg)
    cv2.imwrite('result2.jpg', invNewImg)