#This code tracks your hand and basd on the hands on coordinates sends actuation signals to the Arduino
#Added Serial feedback, better response
import cv2
from cvzone.HandTrackingModule import HandDetector
import serial

linecolor=(0, 255, 0)
thickness = 3

cap = cv2.VideoCapture(1)
detector = HandDetector(detectionCon=0.6)
arduino = serial.Serial('COM11', baudrate = 115200, timeout = 0.1)

while True:
    success, image = cap.read()
    hands, bboxInfo = detector.findHands(image)
    cv2.imshow('image',image)
    if cv2.waitKey(1) & 0xFF == ord('q'):
        break

roi = cv2.selectROI(image)
width = int(roi[2])
height = int(roi[3])
blockwidth = int(width/4)
blockheight = int(height/4)



while True:
    success, image = cap.read()
    image = cv2.flip(image, 2)
    imagecrop = image[int(roi[1]):int(roi[1]+roi[3]), int(roi[0]):int(roi[0]+roi[2])]
    

    
    #imagecrop = cv2.flip(imagecrop, 0)
    hands, bboxInfo = detector.findHands(imagecrop, draw=False, flipType=True)
    if hands:
        hands = hands[0]
        handlandmark = hands["center"]
        x = handlandmark[0]
        y = handlandmark[1]

        if detector.fingersUp(hands) == [1,1,1,1,1]:
            if( 0 < x < 1*blockwidth) & (0 < y < blockheight):
                print("Servo 1")
                arduino.write(b'1')
                val = arduino.readline().decode('utf-8')
                if len(val) > 0:
                    if val[0] == '1':
                        print("Servo 1 Engaged")
            elif( blockwidth < x < 2*blockwidth) & (0 < y < blockheight):
                print("Servo 2")
                arduino.write(b'2')
                val = arduino.readline().decode('utf-8')
                if len(val) > 0:
                    if val[0] == '2':
                        print("Servo 2 Engaged")                
            elif( blockwidth < x < 3*blockwidth) & (0 < y < blockheight):
                print("Servo 3")
                arduino.write(b'3')
                val = arduino.readline().decode('utf-8')
                if len(val) > 0:
                    if val[0] == '3':
                        print("Servo 3 Engaged")
            elif( blockwidth < x < 4*blockwidth) & (0 < y < blockheight):
                print("Servo 4")
                arduino.write(b'4')
                val = arduino.readline().decode('utf-8')
                if len(val) > 0:
                    if val[0] == '4':
                        print("Servo 4 Engaged")                
            elif( 0 < x < 1*blockwidth) & (0 < y < 2*blockheight):
                print("Servo 5")
                arduino.write(b'5')
                val = arduino.readline().decode('utf-8')
                if len(val) > 0:
                    if val[0] == '5':
                        print("Servo 5 Engaged")
            elif( 0 < x < 2*blockwidth) & (0 < y < 2*blockheight):
                print("Servo 6")
                arduino.write(b'6')
                val = arduino.readline().decode('utf-8')
                if len(val) > 0:
                    if val[0] == '6':
                        print("Servo 6 Engaged")
            elif( 0 < x < 3*blockwidth) & (0 < y < 2*blockheight):
                print("Servo 7")
                arduino.write(b'7')
                val = arduino.readline().decode('utf-8')
                if len(val) > 0:
                    if val[0] == '7':
                        print("Servo 7 Engaged")
            elif( 0 < x < 4*blockwidth) & (0 < y < 2*blockheight):
                print("Servo 8")
                arduino.write(b'8')
                val = arduino.readline().decode('utf-8')
                if len(val) > 0:
                    if val[0] == '8':
                        print("Servo 8 Engaged")
            elif( 0 < x < 1*blockwidth) & (0 < y < 3*blockheight):
                print("Servo 9")
                arduino.write(b'9')
                val = arduino.readline().decode('utf-8')
                if len(val) > 0:
                    if val[0] == '9':
                        print("Servo 9 Engaged")
            elif( 0 < x < 2*blockwidth) & (0 < y < 3*blockheight):
                print("Servo 10")
                arduino.write(b'a')
                val = arduino.readline().decode('utf-8')
                if len(val) > 0:
                    if val[0] == 'a':
                        print("Servo 10 Engaged")                
            elif( 0 < x < 3*blockwidth) & (0 < y < 3*blockheight):
                print("Servo 11")
                arduino.write(b'b')
                val = arduino.readline().decode('utf-8')
                if len(val) > 0:
                    if val[0] == 'b':
                        print("Servo 11 Engaged")
            elif( 0 < x < 4*blockwidth) & (0 < y < 3*blockheight):
                print("Servo 12")
                arduino.write(b'c')
                val = arduino.readline().decode('utf-8')
                if len(val) > 0:
                    if val[0] == 'c':
                        print("Servo 12 Engaged")                

            elif( 0 < x < 1*blockwidth) & (0 < y < 4*blockheight):
                print("Servo 13")
                arduino.write(b'd')
                val = arduino.readline().decode('utf-8')
                if len(val) > 0:
                    if val[0] == 'd':
                        print("Servo 13 Engaged")
            elif( 0 < x < 2*blockwidth) & (0 < y < 4*blockheight):
                print("Servo 14")
                arduino.write(b'e')
                val = arduino.readline().decode('utf-8')
                if len(val) > 0:
                    if val[0] == 'e':
                        print("Servo 14 Engaged")
            elif( 0 < x < 3*blockwidth) & (0 < y < 4*blockheight):
                print("Servo 15")
                arduino.write(b'f')
                val = arduino.readline().decode('utf-8')
                if len(val) > 0:
                    if val[0] == 'f':
                        print("Servo 15 Engaged")
            elif( 0 < x < 4*blockwidth) & (0 < y < 4*blockheight):
                print("Servo 16")
                arduino.write(b'g')
                val = arduino.readline().decode('utf-8')
                if len(val) > 0:
                    if val[0] == 'g':
                        print("Servo 16 Engaged")

        elif detector.fingersUp(hands) == [0,0,0,0,0]:
            print("close")
            arduino.write(b'x')
            val = arduino.readline().decode('utf-8')
            if len(val) > 0:
                if val[0] == 'x':
                    print("Servos Reverse")

    #grid
    cv2.line(imagecrop, (blockwidth,0), (blockwidth,height), color=linecolor, thickness=thickness)
    cv2.line(imagecrop, (2*blockwidth,0), (2*blockwidth,height), color=linecolor, thickness=thickness)
    cv2.line(imagecrop, (3*blockwidth,0), (3*blockwidth,height), color=linecolor, thickness=thickness)
    cv2.line(imagecrop, (0,blockheight), (width,blockheight), color=linecolor, thickness=thickness)
    cv2.line(imagecrop, (0,2*blockheight), (width,2*blockheight), color=linecolor, thickness=thickness)
    cv2.line(imagecrop, (0,3*blockheight), (width,3*blockheight), color=linecolor, thickness=thickness)
 

    cv2.imshow('image',imagecrop)
    if cv2.waitKey(1) & 0xFF == ord('q'):
        break
    



cap.release()
cv2.destroyAllWindows()