import io           # detect orange object using pi camera module
import picamera     # at this stage i'm getting ~ 2-3 fps - object x,y screen coordinates/size output
import cv2
import numpy as np
import serial

stream = io.BytesIO()
cam = picamera.PiCamera()                    # open pi camera module 
ser = serial.Serial('/dev/ttyUSB0', 115200)  # open usb connection to propeller

while True:
    cam.capture(stream, use_video_port=True, resize=(110,80), format='jpeg')
    stream.seek(0)
    data = np.fromstring(stream.getvalue(), dtype=np.uint8)
    frame = cv2.imdecode(data, 1)
    frame = cv2.blur(frame,(3, 3))
    hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
    thresh = cv2.inRange(hsv, np.array((5 , 50, 50)), np.array((15, 255, 255)))
    thresh2 = thresh.copy()
    contours, hierarchy = cv2.findContours(thresh, cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE)
    max_area = 0
    for cnt in contours:
        area = cv2.contourArea(cnt)
        if area > max_area:
            max_area = area
            best_cnt = cnt

    if max_area > 0:
        M = cv2.moments(best_cnt)
        cx, cy = int(M['m10'] / M['m00']), int(M['m01'] / M['m00'])
        cz = int(max_area)
        cv2.rectangle(frame, (cx - 5, cy - 5), (cx + 5, cy + 5), (0, 0, 255), 2)
        ser.write('!')
        ser.write('&')
        ser.write(chr(cx))
        ser.write(chr(cy))
        ser.write(chr(cz & 0xff))
        ser.write(chr(cz >> 8))

    cv2.imshow('V', frame)
    cv2.imshow('T', thresh2)
    if cv2.waitKey(1) == 27: # escape key exits
        break

cam.close()
ser.close()
cv2.destroyAllWindows()