import numpy as np
import serial
import cv2

# create video capture, serial port 
cap = cv2.VideoCapture(0)
ser = serial.Serial('/dev/ttyAMA0', 115200)

# main loop
while(1):
    
    # read the current video frame
    _, frame = cap.read()

    # resize captured video frame to smaller size for faster processing
    frame = cv2.resize(frame, (190, 140), interpolation = cv2.INTER_LINEAR)

    # smooth it
    frame = cv2.blur(frame, (3, 3))

    # convert to hsv and find range of colors
    hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
    thresh = cv2.inRange(hsv, np.array((7, 100, 100)), np.array((14, 255, 255)))
    thresh2 = thresh.copy()

    # find contours in the threshold image
    contours, hierarchy = cv2.findContours(thresh, cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE)

    # finding contour with maximum area and store it as best_cnt
    x = 0
    max_area = 0
    for cnt in contours:
        area = cv2.contourArea(cnt)
        if area > max_area:
            max_area = area
            best_cnt = cnt
            x = 1 
       
    # find centroid of best_cnt and draw a circle there, send to serial port
    if x == 1:
        M = cv2.moments(best_cnt)
        cx, cy = int(M['m10']/M['m00']), int(M['m01']/M['m00'])
        cv2.circle(frame, (cx, cy), 4, (255, 0, 0), -1)
        cz = int(max_area)
        ser.write('!')
        ser.write('&')
        ser.write(chr(cx))
        ser.write(chr(cy))
        ser.write(chr(cz & 0xff))
        ser.write(chr(cz >> 8))
#    print cx, cy, cz
    # Show it, if key pressed is 'Esc', exit the loop
    cv2.imshow('Frame', frame)
    cv2.imshow('Threshold', thresh2)
    if cv2.waitKey(1) == 27:
        break

# Clean up everything before leaving
ser.close()
cap.release()
cv2.destroyAllWindows()