import cv
import serial
import time
import threading
import sys


class Target:
    def __init__(self):
        self.capture = cv.CaptureFromCAM(0)
        cv.NamedWindow("Target", 1)

    def run(self):
        # Capture first frame to get size
        frame = cv.QueryFrame(self.capture)
        frame_size = cv.GetSize(frame)
        grey_image = cv.CreateImage(cv.GetSize(frame), cv.IPL_DEPTH_8U, 1)
        moving_average = cv.CreateImage(cv.GetSize(frame), cv.IPL_DEPTH_32F, 3)
        difference = None
        ser = serial.Serial("/dev/ttyACM0", 9600, timeout = 1) #need to uncomment
        connected = 0 # need to change back to 0 after test
        while connected == 0:
            print("not connected") #Just debug message
            ser.write('c') #Seeing if the arduino is open to com yet
            if ser.read() == '4': #Arduino saying it is open to com
                 connected = 1 #Another debug message
                 print("connected")
                 ser = serial.Serial("/dev/ttyACM0", 9600, timeout = .1)
            time.sleep(1)

            print("it has slept")

        while connected == 1:
            print("it worked")
            # Capture frame from webcam
            color_image = cv.QueryFrame(self.capture)

            # Smooth to get rid of false positives
            cv.Smooth(color_image, color_image, cv.CV_GAUSSIAN, 3, 0)

            if not difference:
                # Initialize
                difference = cv.CloneImage(color_image)
                temp = cv.CloneImage(color_image)
                cv.ConvertScale(color_image, moving_average, 1.0, 0.0)
            else:
                cv.RunningAvg(color_image, moving_average, 0.020, None)

            # Convert the scale of the moving average.
            cv.ConvertScale(moving_average, temp, 1.0, 0.0)

            # Minus the current frame from the moving average.
            cv.AbsDiff(color_image, temp, difference)

            # Convert the image to grayscale.
            cv.CvtColor(difference, grey_image, cv.CV_RGB2GRAY)

            # Convert the image to black and white.
            cv.Threshold(grey_image, grey_image, 70, 255, cv.CV_THRESH_BINARY)

            # Dilate and erode to get object blobs
            cv.Dilate(grey_image, grey_image, None, 18)
            cv.Erode(grey_image, grey_image, None, 10)

            # Calculate movements
            storage = cv.CreateMemStorage(0)
            contour = cv.FindContours(grey_image, storage, cv.CV_RETR_CCOMP, cv.CV_CHAIN_APPROX_SIMPLE)
            points = []

            while contour:
                # Draw rectangles
                bound_rect = cv.BoundingRect(list(contour))
                contour = contour.h_next()

                pt1 = (bound_rect[0], bound_rect[1])
                pt2 = (bound_rect[0] + bound_rect[2], bound_rect[1] + bound_rect[3])
                points.append(pt1)
                points.append(pt2)
                cv.Rectangle(color_image, pt1, pt2, cv.CV_RGB(255,0,0), 1)

            num_points = len(points)
            if num_points:
                # Draw bullseye in midpoint of all movements
                x = y = 0
                for point in points:
                    x += point[0]
                    y += point[1]
                x /= num_points
                y /= num_points
                center_point = (x, y)
                cv.Circle(color_image, center_point, 40, cv.CV_RGB(255, 255, 255), 1)
                cv.Circle(color_image, center_point, 30, cv.CV_RGB(255, 100, 0), 1)
                cv.Circle(color_image, center_point, 20, cv.CV_RGB(255, 255, 255), 1)
                cv.Circle(color_image, center_point, 10, cv.CV_RGB(255, 100, 0), 5)
                    #############serial communication###########
                ser.write("x") #checking if arduino is ready for new cordinates
                read = ser.read()
                if read == '3': #arduino saying it's ready for new cordinates
                   ser.write(str(center_point[0]))
                read = ser.read()
                if read == '4':
                   ser.write(str(center_point[1]))
               # print(center_point[1])
                print(ser.readline(len(str(center_point[1]+3))))
                ser.flushInput()
                ser.flushOutput()




            # Display frame to user
            cv.ShowImage("Target", color_image)

            # Listen for ESC or ENTER key
            c = cv.WaitKey(7) % 0x100
            if c == 27 or c == 10:
                ser.close
                break

if __name__=="__main__":
    t = Target()
    t.run()