# import the necessary packages
from __future__ import division
import numpy as np
import cv2
from matplotlib import pyplot as plt

images = [
    [cv2.imread("valentin.png"), "valentin"],
    [cv2.imread("left.png"), "left"],
    #[cv2.imread("right.png"), "right"],
    #[cv2.imread("noway.png"), "noway"],
    #[cv2.imread("up.png"), "uo"]
]

def ToBlackWhite(img):
    height, width = img.shape[:2]

    for i in range(0,height):
        for j in range(0,width):
            # BGR
            if (img[i,j,2] > 2*img[i,j,1] and img[i,j,2] > 2*img[i,j,0]) or (img[i,j,0] > 2*img[i,j,1] and img[i,j,0] > 2*img[i,j,2]):
                img[i,j] = [255,255,255]
            else:
                img[i,j] = [0,0,0];
    return img

def equal(p1, p2):
    return abs(np.int16(p1) - np.int16(p2)) < 10


def calculate(img1, img2):
    count = 0
    height1, width1 = img1.shape[:2]
    height2, width2 = img2.shape[:2]

    img1 = cv2.resize(img1, (width2, height2))


    for w in range(0,width2):
        for h in range(0,height2):
            if equal(img1[h][w], img2[h][w]):
                count = count + 1
    print "calculate", count, "from", width2,width2

    return count/(width2*height2)


#for (lower, upper) in boundaries:
 # lower = np.array(lower, dtype = "uint8")
  #upper = np.array(upper, dtype = "uint8")

  # find the colors within the specified boundaries and apply
  # the mask
  #  mask = cv2.inRange(img1, lower, upper)
  #  output = cv2.bitwise_and(image, image, mask = mask)

def isWhitePixel(image, w, h):
    #print w,h
    #print image[w][h]
    return image[w][h][0] > 200 and image[w][h][1] > 200 and image[w][h][2] > 200

def isLeftTurn(positions):
    dif = 10
    count = 0
    perespect = ((positions[0] - positions[1])+(positions[1] - positions[2]))/2
    dif = dif+perespect
    print perespect
    if positions[0] - positions[1] > dif:
        count = count +1
    if positions[1] - positions[2] > dif:
        count = count +1
    if positions[2] - positions[3] > dif:
        count = count +1
    if positions[3] - positions[4] > dif:
        count = count +1
    if positions[4] - positions[5] > dif:
        count = count +1
    #if count >3:
    print "left", count

def isRightTurn(positions):
    dif = 10
    count = 0
    perespect = ((positions[0] - positions[1])+(positions[1] - positions[2]))/2
    dif = dif + perespect
    if positions[1] - positions[0] > dif:
        count = count +1
    if positions[2] - positions[1] > dif:
        count = count +1
    if positions[3] - positions[2] > dif:
        count = count +1
    if positions[4] - positions[3] > dif:
        count = count +1
    if positions[5] - positions[4] > dif:
        count = count +1
    #if count > 3:
    print "right", count

def isWhite(image, w, h):
    return isWhitePixel(image, w, h) and isWhitePixel(image, w+2, h) and isWhitePixel(image, w+4, h) and isWhitePixel(image, w+6, h) and isWhitePixel(image, w+8, h)

def processRoad(img):
    height, width = img.shape[:2]

    lower = np.array([200, 200, 200], dtype = "uint8")
    upper = np.array([255, 255, 255], dtype = "uint8")
    mask = cv2.inRange(img, lower, upper)
    output = cv2.bitwise_and(image, image, mask = mask)
   # output = cv2.cvtColor(image,cv2.COLOR_BGR2GRAY)
    #output2 = cv2.Canny(output,100,200)
    cv2.imshow("road", output)

    h, w = image.shape[:2]

    positions = []

    for i in range(h-10, 2, -15):
        for j in range(w-10, 2, -15):
            #print i,h,j,w
            if isWhite(output, i, j):
                #print "positions append",j
                positions.append(j)
                break
    if len(positions) > 0:
        print len(positions)
    if len(positions) > 5:
        if isLeftTurn(positions):
            print "left"
        elif isRightTurn(positions):
            print "right"
        else:
            print "none"


# load the image
#image = cv2.imread("img.png")

video_capture = cv2.VideoCapture(1)
cv2.startWindowThread()
cv2.namedWindow("preview")

for idx, image in enumerate(images):
    img = ToBlackWhite(image[0].copy())
    img = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)
    img = cv2.Canny(img,100,200)
    images[idx] = [img, image[1]]



# loop over the boundaries
while True:


    ret, image = video_capture.read()
    h, w = image.shape[:2]
    image = cv2.resize(image, (int(w/4), int(h/4)))
    cv2.waitKey(1)

    #processRoad(image.copy())
    #continue

    image = ToBlackWhite(image.copy())



    output = cv2.cvtColor(image,cv2.COLOR_BGR2GRAY)
    output2 = cv2.Canny(output,100,200)


    im, contours,hierarchy = cv2.findContours(output2.copy(),cv2.RETR_LIST,cv2.CHAIN_APPROX_SIMPLE)
    print len(contours)
    bigger_size = 0
    biggest_cnt = None
    for cnt in contours:
        x,y,w,h = cv2.boundingRect(cnt)
        if w*h > bigger_size:
            bigger_size = w*h
            biggest_cnt = cnt
            print "size",w*h,"pos",x,y

    if biggest_cnt is not None:
        x,y,w,h = cv2.boundingRect(biggest_cnt)
        #cv2.rectangle(output2,(x-1,y-1),(x+w+2,y+h+2),(0,255,0),2)

        cv2.imshow("preview", output2)

        best_res = 0
        best_image = "noimage"

        for image in images:
            res = calculate( output2[y: y+h,x: x+w], img)
            if res > best_res:
                best_image = image[1]
                best_res = res

        print "best match", best_image