import cv2
import mediapipe as mp
mp_drawing = mp.solutions.drawing_utils
mp_hands = mp.solutions.hands
from math import sqrt

def calculate_distance(ax, ay, bx,by):
  distance = sqrt(((bx - ax) ** 2 + (by - ay) ** 2))
  return distance



hands = mp_hands.Hands(
    min_detection_confidence=0.5, min_tracking_confidence=0.5)
cap = cv2.VideoCapture(0)
while cap.isOpened():
  success, image = cap.read()

  # Flip the image horizontally for a later selfie-view display, and convert
  # the BGR image to RGB.
  image = cv2.cvtColor(cv2.flip(image, 1), cv2.COLOR_BGR2RGB)
  # To improve performance, optionally mark the image as not writeable to
  # pass by reference.
  image.flags.writeable = False
  results = hands.process(image)


  # Draw the hand annotations on the image.g
  image.flags.writeable = True
  image = cv2.cvtColor(image, cv2.COLOR_RGB2BGR)
  if results.multi_hand_landmarks:
    for hand_landmarks in results.multi_hand_landmarks:
      mp_drawing.draw_landmarks(
          image, hand_landmarks, mp_hands.HAND_CONNECTIONS)
      #print(hand_landmarks)        ## spójrz na to
  if cv2.waitKey(33) == ord('s'):
    if results.multi_hand_landmarks:
      i = 0
      for hand_landmarks in results.multi_hand_landmarks:
        ax = hand_landmarks.landmark[8].x
        ay = hand_landmarks.landmark[8].y
        bx = hand_landmarks.landmark[5].x
        by = hand_landmarks.landmark[5].y
        odleglosc_8_5 = calculate_distance(ax,ay, bx, by)
        print(odleglosc_8_5)
        ax = hand_landmarks.landmark[5].x
        ay = hand_landmarks.landmark[5].y
        bx = hand_landmarks.landmark[0].x
        by = hand_landmarks.landmark[0].y
        odleglosc_5_0 = calculate_distance(ax, ay, bx, by)
        print(odleglosc_5_0)
        if(odleglosc_5_0 < odleglosc_8_5 + 0.1):
          print("wyprostowany")
        else:
          print("niewyprostowany")


        # i += 1
        # print(hand_landmarks.)

  cv2.imshow('MediaPipe Hands', image)
  if cv2.waitKey(5) & 0xFF == 27:
    break
hands.close()
cap.release()