from cv2 import cv2
import numpy as npy
import serial
from time import sleep

port = "COM23"
baud = 9800
ser = serial.Serial(port, baud, timeout=1)

a = input()

while (a != 's'):
    buf = b'\x08\x00\x80\x09\x00\x04' + bytes([0]) + bytes([0]) + bytes([1]) + bytes([0])
    ser.write(buf)
    print(ser.readline())
    a = input()


buf = b'\x08\x00\x80\x09\x00\x04' + bytes([0]) + bytes([0]) + bytes([2]) + bytes([0])
ser.write(buf)

cap = cv2.VideoCapture(0)
cv2.namedWindow('image')

low1 = npy.array([0, 130, 90])
high1 = npy.array([15, 255, 255])
low2 = npy.array([165, 130, 90])
high2 = npy.array([180, 255, 255])

low3 = npy.array([69, 100, 120])
high3 = npy.array([75, 200, 220])

encoder = 0

berries = [[],[]]
allberries = []

switch = 0
lastid = -1
# 0 130 90
# 165 130 90

while(True):
    # Capture frame-by-frame
    ret, frame = cap.read()

    hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)

    mask = cv2.inRange(hsv, low1, high1)
    mask += cv2.inRange(hsv, low2, high2)
    #mask += cv2.inRange(hsv, low3, high3)

    cv2.bitwise_and(hsv, hsv, mask = mask)
    connectivity = 7
    output = cv2.connectedComponentsWithStats(mask, connectivity, cv2.CV_32S)

    num_labels = output[0]
    labels = output[1]
    stats = output[2]
    centroids = output[3]


    del berries[(switch + 1)%2][:]
    for i in range(num_labels):
        x, y, w, h, s = stats[i]
        if s > 2500 and s < 50000 and y > 50 and y < 100:
            sx = hex(x)[2:].zfill(4)
            sy = hex(y)[2:].zfill(4)

            cv2.rectangle(frame,(x,y),(x+w,y+h),(0,0,255),3)
            cv2.circle(frame,(int(centroids[i][0]), int(centroids[i][1])), 3, (0,255,0), -1)

            a = 2500
            b = -1


            for berry in berries[(switch)%2]:

                r = (berry['x'] - centroids[i][0])**2 + (berry['y'] - centroids[i][1])

                if r < a:
                    a = r
                    b = berry['id']



            if b == -1:
                lastid+=1
                berries[(switch + 1)%2].append({'x' : centroids[i][0], 'y' : centroids[i][1], 'id' : lastid})

                a = ser.readline()
                while (len(a) < 7) :
                    timeout = 0
                    buf = b'\x08\x00\x80\x09\x00\x04' + bytes([0]) + bytes([0]) + bytes([3]) + bytes([0])
                    ser.write(buf)
                    a = ser.readline()
                    print(a)

                allberries.append({'x' : centroids[i][0], 'y' : centroids[i][1], 'id' : lastid, 'enc' : a[5] + a[6]*256})
            else :
                berries[(switch + 1)%2].append({'x' : centroids[i][0], 'y' : centroids[i][1], 'id' : b})


    print(allberries)
    switch=(switch + 1)%2








    cv2.imshow('frame', frame)
    cv2.imshow('mask', mask)
    if cv2.waitKey(1)  == ord('q'):
        cap.release()
        cv2.destroyAllWindows()
        exit()