import cv2
import sys, time
from adafruit_servokit import ServoKit
import mediapipe as mp
import math
kit = ServoKit(channels=16)
servo0 = 90
servo1 = 90
servo2 = 90
servo3 = 90
servo4 = 90

kit.servo[0].angle = servo0
kit.servo[1].angle = servo1
kit.servo[2].angle = servo2
kit.servo[3].angle = servo3
kit.servo[4].angle = servo4

mp_drawing = mp.solutions.drawing_utils
mp_hands = mp.solutions.hands
drawing_spec = mp_drawing.DrawingSpec(thickness=1, circle_radius=1)
hands = mp_hands.Hands(static_image_mode=False,
                        max_num_hands=1,
                        min_detection_confidence=0.8)

# Calculate Euclidean distance between two landmarks
def calculate_distance(landmark1, landmark2):
    x1, y1, z1 = landmark1.x, landmark1.y, landmark1.z
    x2, y2, z2 = landmark2.x, landmark2.y, landmark2.z
    distance = math.sqrt((x1 - x2)**2 + (y1 - y2)**2 + (z1 - z2)**2)
    return distance

# Map a value from one range to another
def map_value(value, from_min, from_max, to_min, to_max):
    return (value - from_min) / (from_max - from_min) * (to_max - to_min) + to_min

def get_hand_landmarks(image):
    results = hands.process(cv2.cvtColor(image, cv2.COLOR_BGR2RGB))
    # Print and draw hand mesh landmarks on the image.
    if not results.multi_hand_landmarks:
        return image
    annotated_image = image.copy()
    for hand_landmarks in results.multi_hand_landmarks:
        # uncomment for live printout of all coordiates
        # print(' face_landmarks:', face_landmarks)
        mp_drawing.draw_landmarks(
            image=annotated_image,
            landmark_list=hand_landmarks,
            connections=mp_hands.HAND_CONNECTIONS,
            landmark_drawing_spec=drawing_spec,
            connection_drawing_spec=drawing_spec)
        # uncomment for printout of total number of processed landmarks
        # print('%d facemesh_landmarks' % len(face_landmarks.landmark))
        if len(hand_landmarks.landmark) >= 2:
            # Setup compared landmark couples 
            # thumb
            landmark1 = hand_landmarks.landmark[1]
            landmark2 = hand_landmarks.landmark[5]
            # pointer
            landmark3 = hand_landmarks.landmark[5]
            landmark4 = hand_landmarks.landmark[8]
            # middle
            landmark5 = hand_landmarks.landmark[9]
            landmark6 = hand_landmarks.landmark[12]
            # ring
            landmark7 = hand_landmarks.landmark[13]
            landmark8 = hand_landmarks.landmark[16]
            # pinky
            landmark9 = hand_landmarks.landmark[17]
            landmark10 = hand_landmarks.landmark[20]            

            # Calculate the distance between the two landmarks
            distance0 = calculate_distance(landmark1, landmark2)
            distance1 = calculate_distance(landmark3, landmark4)
            distance2 = calculate_distance(landmark5, landmark6)
            distance3 = calculate_distance(landmark7, landmark8)
            distance4 = calculate_distance(landmark9, landmark10)
   
            # print(distance0)            
            if distance0<0.09:
                distance0=0.09
            if distance0>0.12:
                distance0=0.12    
            # Map the distance to servo angle (adjust the mapping values as needed)
            servo_angle0 = int(map_value(distance0, 0.09, 0.12, 0, 160))
            print(servo_angle0)
            # Set servo angle
            kit.servo[0].angle = servo_angle0

            # print(distance1) 
            if distance1<0.09:
                distance1=0.09
            if distance1>0.12:
                distance1=0.12    
            servo_angle1 = int(map_value(distance1, 0.09, 0.12, 0, 160))
            #print(servo_angle1)
            kit.servo[1].angle = servo_angle1

            # print(distance2) 
            if distance2<0.25:
                distance2=0.25
            if distance2>0.34:
                distance2=0.34    
            servo_angle2 = int(map_value(distance2, 0.25, 0.34, 0, 160))
            # print(servo_angle2)
            kit.servo[2].angle = servo_angle2

            # print(distance3) 
            if distance3<0.25:
                distance3=0.25
            if distance3>0.34:
                distance3=0.34    
            servo_angle3 = int(map_value(distance3, 0.25, 0.34,  0, 160))
            # print(servo_angle3)
            kit.servo[3].angle = servo_angle3

            # print(distance4) 
            if distance4<0.08:
                distance4=0.08
            if distance4>0.1:
                distance4=0.1    
            servo_angle4 = int(map_value(distance4, 0.08, 0.1,  0, 160))
            # print(servo_angle4)
            kit.servo[4].angle = servo_angle4

    return annotated_image

font = cv2.FONT_HERSHEY_SIMPLEX
cap = cv2.VideoCapture(0)
cap.set(cv2.CAP_PROP_FRAME_WIDTH, 640)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)

if (cap.isOpened() == False):
    print("Unable to read camera feed")
while cap.isOpened():
    s = time.time()
    ret, img = cap.read()
    if ret == False:
        print('WebCAM Read Error')
        sys.exit(0)

    annotated = get_hand_landmarks(img)

    #you can do the same for the other landmarks and servos.

    e = time.time()
    fps = 1 / (e - s)
    cv2.putText(annotated, 'FPS:%5.2f' % (fps), (10, 50), font, fontScale=1, color=(0, 255, 0), thickness=1)
    cv2.imshow('webcam', annotated)
    key = cv2.waitKey(1)
    if key == 27:  # ESC
        break
cap.release()