import cv2
import numpy as np 
import uart
from buffer import Buffer
import pwm
import time
import threading
Height=240
Width=320
targets=Buffer(3)
FLY_UP=1
FLY_DOWN=3
GO_DOWN=4
cmd=0
# cv2.namedWindow("win", cv2.WINDOW_NORMAL)
def threadfunction():
    global cmd
    t=10
    cmd=FLY_UP
    print("change cmd:forward")
    time.sleep(t)
    cmd=0
    pwm.open()
    print("put")
    time.sleep(t/2)
    cmd=FLY_DOWN
    print("change cmd:back")
    cmd=GO_DOWN

    print("over")


def find_bucket(img)->tuple:
    
    frame = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    # frame = cv2.medianBlur(frame,3)
    # contrast = 1.2
    # brightness = 0.5
    # frame = cv2.convertScaleAbs(frame, alpha=contrast, beta=brightness)
    # frame = cv2.medianBlur(frame,11)
    frame = cv2.GaussianBlur(frame,(17,17),1)
    # frame = cv2.Canny(frame,16,64)
    # kernel = np.ones((4, 4), np.uint8)
    # frame = cv2.dilate(frame, kernel, iterations = 1)
    circle = cv2.HoughCircles(frame, cv2.HOUGH_GRADIENT, 1, 100,param1=20, param2=50, minRadius=3, maxRadius=30)
    res=(None,None)
    if(circle is not None):
        for i in circle[0, :]:  # 遍历矩阵的每一行的数据
            # 绘制圆形
            cv2.circle(frame, (int(i[0]), int(i[1])), int(i[2]), (255, 0, 0), 5)
            # 绘制圆心
            cv2.circle(frame, (int(i[0]), int(i[1])), 5, (255, 0, 0), -1)
            res=(int(i[0]), int(i[1]))
    cv2.imshow("win",frame)
    return res


none_count=0
yes_count=0
# cam=cv2.VideoCapture(0)
pwm.close()
task=threading.Thread(name="task",target=threadfunction)
task.start()
put_flag=False
while(True):
    if(cmd==FLY_UP):
        target=(0,80)
        # print("forward")
        uart.sendTarget(target)
    elif(cmd==FLY_DOWN):
        # if(put_flag==False):
        #     # pwm.open()
        #     # print("put")
        #     pass
        #     put_flag=True
        target=(0,-80)
        # print("back")
        uart.sendTarget(target)
    elif(cmd==GO_DOWN):
        # print("down")
        uart.sendcmd(7)
    elif(cmd==0):
        uart.sendTarget((None,None))
    
    

# if(cam.isOpened()):
#     while(True):
#         _,frame=cam.read()
#         # frame=cv2.imread("test/3.png")
#         target=find_bucket(frame)
#         if(target[0]!=None):
#             none_count=0
#             target=(target[0]-Width/2,Height/2-target[1])
#             targets.add(target)
#             if(abs(target[0])+abs(target[1])>40):
#                 yes_count+=1
#             if(yes_count>20):
#                 pwm.open()
#                 print("out!!!")
#                 uart.sendcmd(3)
#                 break
#             print(target)
#         else:
#             none_count+=1
           
#             if(none_count>3):
#                 target=(None,None)
#                 yes_count=0
#             else:
#                target=targets.GetAve()
#         uart.sendTarget(target)

#         if cv2.waitKey(1) & 0xFF == ord("q"):
#             break