import cv2
from base_serial import SerialWorker

# Import the seria Worker
serial = SerialWorker()
serial.conectar("/dev/ttyACM0")

# Loads the default people detection model
hog = cv2.HOGDescriptor()
hog.setSVMDetector(cv2.HOGDescriptor.getDefaultPeopleDetector())

# Capture video on Webcam (default)
video_capture = cv2.VideoCapture(0)
tracker = None

while True:
    ret, frame = video_capture.read()
    if not ret: break

    # Size of the video frame
    frame = cv2.resize(frame, (640, 480))

    # Creates the tracking bounding box on the person
    if tracker is None:
        (rects, weights) = hog.detectMultiScale(frame, winStride=(4, 4), padding=(8, 8), scale=1.05)
        for (x, y, w, h) in rects:
            try:
                # Assigns a tracker to the person
                tracker = cv2.TrackerCSRT_create()
            except AttributeError:
                tracker = cv2.legacy.TrackerCSRT_create()
            
            tracker.init(frame, (x, y, w, h))
            break # Solo trackeamos a la primera persona detectada

    # Updates position of the bounding box as the person moves
    if tracker is not None:
        (success, box) = tracker.update(frame)
        if success:
            (x, y, w, h) = [int(v) for v in box]
            cv2.rectangle(frame, (x, y), (x + w, y + h), (255, 0, 0), 2)
            
            # Calculates the center of the image
            centro_x = x + (w // 2)
            
            # Sending the value to the RP2350
            serial.send(str(centro_x))
        else:
            # Re track if the person is lost
            tracker = None

    cv2.imshow('Tracking para Motor', frame)
    if cv2.waitKey(1) & 0xFF == ord('q'):
        break

video_capture.release()
cv2.destroyAllWindows()