# import the necessary packages
from collections import deque
import numpy as np
import argparse
import imutils
import cv2
import serial
import time
import re
import sys

class Comport:
    def __init__(self,com,speed):
        self.comport = com
        self.comspeed = speed

    def opencom(self):
        self.serial = serial.Serial(port=self.comport,baudrate=self.comspeed,timeout=5)

    def writeln(self,str):
        l_block = str.strip()
        self.serial.write(l_block + '\n')
        #print('writing : {}'.format(str))
        #self.serial.write(str)
        #self.serial.write(num)

    def readline(self):
        return self.serial.readline()

    def closecom(self):
        self.serial.close()
        print "closing COMport"

ap = argparse.ArgumentParser()
ap.add_argument("-v", "--video", help="path to video")
ap.add_argument("-b", "--buffer", type=int, default=64,help="maxbuffsize")
args = vars(ap.parse_args())

colorLower = (85, 115, 83)
colorUpper = (133, 255, 215)
pts = deque(maxlen=args["buffer"])
a = Comport("/dev/ttyACM0", 115200)
a.opencom()


if not args.get("video", False):
    camera = cv2.VideoCapture(0)
else:
    camera = cv2.VideoCapture(args["video"])

while True:
    (grabbed, frame) = camera.read()

    if args.get("video") and not grabbed:
        break

    frame = imutils.resize(frame, width=600)
    hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)

    mask = cv2.inRange(hsv, colorLower, colorUpper)
    mask = cv2.erode(mask, None, iterations=2)
    mask = cv2.dilate(mask, None, iterations=2)
    cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2]
    center = None

    if len(cnts) > 0:
        c = max(cnts, key = cv2.contourArea)
        ((x , y), radius) = cv2.minEnclosingCircle(c)
        M = cv2.moments(c)
        center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))
        print ((x,y))
        #a.writeln(str(float(x)))
        if x >= 300:
            a.writeln("dx")
            #time.sleep(.3)

        elif x < 300:
            a.writeln("sx")
            #time.sleep(.3)

        if radius > 10:
            cv2.circle(frame, (int(x), int(y)), int(radius), (0,255,255), 2 )
            cv2.circle(frame, center, 5, (0,0,255), -1)

    pts.appendleft(center)
    #print(center)

    for i in xrange(1, len(pts)):
        if pts [i -1] is None or pts[i] is None:
            continue

        thickness = int(np.sqrt(args["buffer"] / float(i + 1)) * 2.5)
        cv2.line(frame, pts[i -1], pts[i], (0,0,255), thickness)



    cv2.imshow("Frame", frame)
    key = cv2.waitKey(1) & 0xFF

    if key == ord("q"):
        break
a.closecom()
camera.release()
cv2.destroyAllWindows()