#This code tracks your hand and basd on the hands on coordinates sends actuation signals to the Arduino

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])] 
    #cv2.line(imagecrop, (blockwidth,0), (blockwidth,height), color=linecolor, thickness=thickness) 
    #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')
            elif( blockwidth < x < 2*blockwidth) & (0 < y < blockheight):
                print("servo 2")
                arduino.write(b'2')
            elif( blockwidth < x < 3*blockwidth) & (0 < y < blockheight):
                print("servo 3")
                arduino.write(b'3')
            elif( blockwidth < x < 4*blockwidth) & (0 < y < blockheight):
                print("servo 4")
                arduino.write(b'4')
            elif( 0 < x < 1*blockwidth) & (0 < y < 2*blockheight):
                print("servo 5")
                arduino.write(b'5')
            elif( 0 < x < 2*blockwidth) & (0 < y < 2*blockheight):
                print("servo 6")
                arduino.write(b'6')
            elif( 0 < x < 3*blockwidth) & (0 < y < 2*blockheight):
                print("servo 7")
                arduino.write(b'7')
            elif( 0 < x < 4*blockwidth) & (0 < y < 2*blockheight):
                print("servo 8")
                arduino.write(b'8')
            elif( 0 < x < 1*blockwidth) & (0 < y < 3*blockheight):
                print("servo 9")
                arduino.write(b'9')
            elif( 0 < x < 2*blockwidth) & (0 < y < 3*blockheight):
                print("servo 10")
                arduino.write(b'a')
            elif( 0 < x < 3*blockwidth) & (0 < y < 3*blockheight):
                print("servo 11")
                arduino.write(b'b')
            elif( 0 < x < 4*blockwidth) & (0 < y < 3*blockheight):
                print("servo 12")
                arduino.write(b'c')
            elif( 0 < x < 1*blockwidth) & (0 < y < 4*blockheight):
                print("servo 13")
                arduino.write(b'd')
            elif( 0 < x < 2*blockwidth) & (0 < y < 4*blockheight):
                print("servo 14")
                arduino.write(b'e')
            elif( 0 < x < 3*blockwidth) & (0 < y < 4*blockheight):
                print("servo 15")
                arduino.write(b'f')
            elif( 0 < x < 4*blockwidth) & (0 < y < 4*blockheight):
                print("servo 16")
                arduino.write(b'g')

        elif detector.fingersUp(hands) == [0,0,0,0,0]:
            print("close")
            arduino.write(b'x')
 
    #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()