import cv2
import mediapipe as mp
import numpy as np
import time
import serial
#import struct

mp_haende = mp.solutions.hands
hand = mp_haende.Hands(max_num_hands=1)
mp_markierung = mp.solutions.drawing_utils
kamera = cv2.VideoCapture(0)
serialcom = serial.Serial('COM3', 115200)

lr = 0
ou = 0
vh = 0
zero = 0
geschlossenint = 0
aktive_bewegung = False
letzte_zeit = time.time()
alte_pos = None

min_bewegung = 0.005
fingerabstand_geschlossen = 0.075

while True:
    success, frame = kamera.read()
    if not success:
        break

    frame_rgb = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
    erkennung = hand.process(frame_rgb)

    if erkennung.multi_hand_landmarks:
        for hand_punkte in erkennung.multi_hand_landmarks:
            mp_markierung.draw_landmarks(frame, hand_punkte, mp_haende.HAND_CONNECTIONS)

            handgelenk_cords = hand_punkte.landmark[mp_haende.HandLandmark.WRIST]
            aktuelle_pos = np.array([handgelenk_cords.x, handgelenk_cords.y, handgelenk_cords.z])

            daumen = hand_punkte.landmark[mp_haende.HandLandmark.THUMB_TIP]
            zeigefinger = hand_punkte.landmark[mp_haende.HandLandmark.INDEX_FINGER_TIP]

            daumen_zeigefinger_entfernung = np.linalg.norm(
                np.array([daumen.x, daumen.y, daumen.z]) - np.array([zeigefinger.x, zeigefinger.y, zeigefinger.z])
            )

            geschlossen = True if daumen_zeigefinger_entfernung < fingerabstand_geschlossen else False

            if alte_pos is not None: 
                bewegung = aktuelle_pos - alte_pos
                bewegungs_linie = np.linalg.norm(bewegung)

                aktive_bewegung = bewegungs_linie > min_bewegung                        # Wenn ein Mindestweg (gespeichert in min_bewegung) zurückgelegt wird, wird die boolean aktive_bewegung wahr. Damit sollen ungewollte Eingaben durch kleine Zitterer verhindert werden.

                

                if aktive_bewegung == True:                                             # Wenn sich die Position horizontal verändert hat wird geschaut ob nach links (int lr = 2) oder nach rechts (lr = 1)
                    lr = 2 if bewegung[0] > 0  else 1
                else:
                    lr = 0                                                              # Wenn sich die Position horizontal nicht verändert hat wird lr 0 gesetzt
                    
                if aktive_bewegung == True:
                    ou = 2 if bewegung[1] > 0 else 1
                else:
                    ou = 0

                if aktive_bewegung == True:
                    vh = 2 if bewegung[2] > 0 else 1
                else: 
                    vh = 0
            if geschlossen == True:
                geschlossenint = 1
            else:
                geschlossenint = 2

            #daten = [geschlossenint, 0, vh, ou, ou, lr] 
            #daten = [0, vh, ou, ou, lr, 0]
            #daten = [0, 0, 0, ou, lr, 0]
            daten = [geschlossenint,0,ou,ou,vh,lr]
            aktuelle_zeit = time.time()
            if aktuelle_zeit - letzte_zeit > 0.005:
                serialcom.write(bytearray(daten))
                print(f"Gesendet: {daten}")
                print("Geschlossen: ", geschlossen)
                print("Links-rechts: ", lr)
                print("Oben-unten: ", ou)
                print("Vorne-hinten: ", vh)

            letzte_zeit = aktuelle_zeit
            alte_pos = aktuelle_pos
    cv2.imshow("Handtracking", frame)
    if cv2.waitKey(1) & 0xFF == ord('q'):
        break

kamera.release()
cv2.destroyAllWindows()
hand.close()
serialcom.close()